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■ 
This  dissertation  addresses  the  problem  of  finding  the  desired 

optimal  trajectories  and  the  optimal  control  inputs  for  a  rigid  multi- 
degrees  -of  -freedom  robotic  manipulator  in  a  positioning  mode.   For  a 
general,  highly  coupled  and  nonlinear  dynamic  model  an  analytical  control 
law  is  derived  and  structures  of  controllers  are  suggested . 

The  equations  of  motion  of  a  multilink  mechanism  and  its  actuators 
are  unified  into  a  common  dynamic  model.   A  nonlinear  feedback  trans- 
formation which  globally  linearizes,  decouples  and  places  the  poles  of 
the  closed  loop  system  is  derived.   Each  of  the  decoupled  subsystems 
represents  a  single  degree  of  freedom  of  the  manipulator  motion.   This 
transformation  is  realized  by  on-line  computations  based  on  the  measure- 
ments of  the  manipulator's  state  variables  and  reference  to  a  look-up 
table,  and  requires  no  matrix  inversion. 

vi 


The  set  of  uncoupled  subsystems  in  a  form  of  triple  integrators 
allows  a  solution  of  various  optimal  control  problems  for  each  degree 
of  freedom  independently . 

The  time  optimal  positioning  problem  of  a  multi-degrees-of -freedom 
robotic  manipulator  with  bounded  velocity,  acceleration  and  jerk  is 
posed  in  a  form  which  guarantees  unigueness  of  the  control  and  state 
trajectories .   The  solution  is  demonstrated  via  single  and  double  link 
examples . 

The  look-up  table  implementing  the  feedback  transformation  intro- 
duces non-analytic  (quantization )  errors.   The  time  optimal  trajectories 
of  a  double  link  manipulator  are  shown  to  be  rather  insensitive  to  this 
type  of  error.   For  a  class  of  errors  an  existence  theorem  of  the  linear 
feedback  controller  is  proven  which  guarantees  closed  loop  asymptotic 
stability  at  the  state  space  origin.   Finally,  outlines  of  various 
controllers  structures  are  discussed. 

This  dissertation  is  the  first  application  of  the  following  principles: 
Instead  of  optimizing  the  real  system,  a  fictitous  system  should  be 
introduced  which  is  more  convenient  for  optimization  and  can  be 
obtained  from  the  original  system,  for  example  by  a  state  space 
transformation . 

The  state  space  feedback  transformation  can  be  realized  for  de- 
coupling, linearization  and  pole-placement  simultaneously. 
The  programmed  open  loop  control  for  a  deterministic  system  can  be 
devised  with  the  assumption  that  deviations  are  compensated  by 
a  separated  subsystem  for  "control  in-the-small . " 
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CHAPTER  I 
INTRODUCTION  AND  BACKGROUND 

This  work  deals  with  the  time  optimal  positioning  control  of  robotic 
manipulators.   These  are  general  purpose, programmable  machines  possessing 
certain  anthropomorphic  characteristics.   The  robot  manipulator  system 
is  defined  as  "a  programmable,  multifunction  manipulator  designed 
to  move  material,  parts,  tools  or  specialized  devices  through  variable 
programmed  motions  for  the  performance  of  a  variety  of  tasks . "   In  the 
literature  the  terms  mechanical  arm,  artificial  hand,  robotic  arm,  open 
loop  articulated  chain  and  manipulator  are  used  interchangeably. 

Although  practical  examples  in  this  dissertation  involve  serial 
mechanisms,  much  of  the  theory  developed  applies  to  other  programmable, 
multifunction  chain  structures,  designed  to  provide  purposeful  mechanical 
motion . 

The  robot  manipulator  system  is  composed  of   N  rigid  links,  con- 
nected in  series  by  kinematic  joints,  into  an  open  loop  kinematic  chain. 
The  joints  are  rotary  or  linear  (sliding),  with  the  first  joint  fastened 
to  a  fixed  support  (base),  whereas  the   N-th   link  functions  as  the  hand, 
or  "end -effector, "  which  usually  has  a  gripper  for  performing  a  task. 
Each  joint  is  assumed  to  have  only  one  degree  of  freedom  (DOF);  i.e.,  each 
joint  defines  a  joint  axis  around,  or  along  which,  a  link  rotates  or 
slides.   The  number  of  joints  defines  the  number  of  degrees  of  freedom  of 


the  robot  manipulator.   The  case  of  multiple  rotational  or  combined 
sliding/rotating  degrees  of  freedom  joints  can  be  modelled  by  an  eguiva- 
lent  chain  of  zero  masses  and  lengths  of  single  degree  of  freedom  joints . 
The  standard  dexterity  or  articulation  reguirement  is  six  DOF,  three 
DOF  for  linear  and  three  DOF  for  angular  locating  of  the  end  effector 
with  respect  to  a  fixed  reference,  in  which  case  the  robot  manipulator  is 
called  nonredundant.   If   N,  the  total  number  of  DOF  is  greater  than 
six,  the  manipulator  is  redundant.   The  analysis  attempted  here  is  to  be 
egually  related  to  redundant  as  well  as  nonredundant  manipulators . 

The  description  of  the  manipulator ■ s  state  can  be  provided  either  as 
a  vector  specifying  the  various  joint  angles  or  as  a  position  and  orienta- 
tion of  the  end  effector.   The  former  representation  is  called  configura- 
tion and  is  said  to  be  a  representation  in  joint  variables  (coordinates) 
space  or  joint  space.   The  latter  is  a  representation  in  cartesian  space. 
The  locus  of  time  dependent  points  which  the  manipulators '  joints  trace 
is  called  a  trajectory. 

In  the  analysis  of  robotic  manipulators,  two  main  problems  can  be 
formulated.   In  the  first,  the  desired  position  and  orientation  of  the 
end  effector  are  given.   The  joints'  positions  which  will  locate  the  hand 
at  the  desired  point  in  the  cartesian  space  with  the  specified  orientation 
are  to  be  determined.   For  six  DOF  manipulators  (nonredundant)  the  number 
of  solutions  to  this  problem  is  finite  and  may  be  obtained  (except  at 
singular  points,  [S-5])  by  inversion  of  the  geometrical  transformation 
which  relates  the  joint  variables  to  the  hand's  position  and  orientation. 
For  a  redundant  spatial  N-DOF  manipulator  there  may  be  infinitely  many 
solutions.   Throughout  this  work,  however,  we  assume  that  an  appropriate 
solution  for  this  problem  exists  and  is  feasible. 


The  second  problem  is  one  of  manipulators  dynamic  control:   find  the 
structure  of  the  controller  and  the  controls  (inputs)  which  provide  the 
movement  of  the  manipulator  system  from  its  present  configuration  to  the 
desired  one,  such  that  constraints  on  the  hand's  and  joint's  path,  velocity, 
acceleration  and  jerk  as  well  as  actuators  losses  and  energy  consumption 
are  met.   If  the  motion  execution  is  optimal  with  respect  to  some  given 
"goodness  criteria"  then  it  is  called  an  optimal  control  problem. 

The  basic  tasks  performed  by  industrial  manipulators  may  usually  be 
reduced  to  two  types,  regardless  of  the  complexity  of  the  manipulation  task. 
In  the  first,  sometimes  called  point  to  point  task,  the  cartesian 
path  of  the  manipulator's  hand  or  joints  is  not  specified  and  may  fre- 
quently be  arbitrarily  chosen.   Typical  point  to  point  applications  include 
"pick  and  place"  activities,  machine  loading  and  unloading,  spot  welding, 
etc.   The  second  type  of  tasks  performed  by  manipulators  is  sometimes 
called  tracking  or  "contouring."   This  type  includes  applications  such  as 
paint  spraying,  continuous  welding  processes,  some  metal  -cutting  operation 
and  grasping  objects  moving  on  a  conveyor.   In  this  mode  the  desirable 
trajectory  of  the  motion  is  preassigned  and  is  an  essential  part  of  the 
manipulator's  task  definition. 

In  the  point  to  point  task  type,  the  trajectory  leading  to  the 
desired  terminal  point  is  frequently  of  no  importance  (except  of  obstacle 
avoidance  requirement),  e.g.,  the  motion  of  an  empty  gripper  towards  a 
tool  or  object.   In  this  type  of  task,  optimization  procedures  may  be 
applied  to  the  control  synthesis:   minimization  of  time  to  reach  the 
desired  state,  optimal  velocity  distribution  along  the  path,  minimization 


of  energy  used  and  dissipated  by  actuators  which  impose  constraints  to 

the  control,  etc.   The  present  work  is  aimed  mainly  at  this  class  of  tasks. 

An  observation  made  by  many  researchers  (e.g.,  [B-2],  [B-5],  [L-4],  [T-2] 
and  [V-1  ] )  is  that  in  the  point  to  point  task  the  manipulator  motion  should 
be  partitioned  into  two  modes:  "large,"  and  "small."   The  first,  which  is 
referred  to  as  "slewing  mode"  [V-1],  "coarse  motion"  [B-5],  "transport  phase" 
[B-2],  or  "gross  motion"  [L-4],  is  the  main  part  of  the  motion  made  as  the 
manipulator  transports  objects  or  tools  from  one  point  to  another  in  the 
work  space.   At  this  part,  fast  motion  for  higher  productivity  is  to  be 
emphasized  whereas  accuracy  seems  to  be  less  important.   The  second  mode 
which  is  denoted  as  "fine  motion"  [L-4],  [B-5],  or  "terminal  phase"  [B-2], 
is  the  part  of  the  task  where  the  system  is  exerting  a  force,  manipulating 
or  having  contact  with  external  objects,  with  a  relatively  slow  motion  (if 
at  all)  of  most  parts,  performing  "rendezvous,"  "accurate-stop"  or  fine 
insertions.   Since,  in  this  work,  we  are  dealing  primarily  with  time 
optimization  of  the  manipulator's  motion,  the  slewing  mode  (or  the  coarse 
positioning)  is  naturally  our  main  concern,  which  actually  implies  the 
possibility  to  base  our  research  upon  a  deterministic  model . 

In  terms  of  the  complexity  of  the  control  analysis,  the  coarse  and 
fine  modes  of  motion  differ  significantly.   If  the  system  is  to  be  moved 
slowly,  or  by  varying  one  joint  at  a  time  (as  in  the  fine  mode),  then 
coupling  torgues,  such  as  the  Coriolis  and  centrifugal  terms,  are  minimal, 
and  a  simple  solution  to  the  control  problem  for  each  of  the  joints  can 
be  found  independently.   However,  in  the  slewing  mode  the  motion  is  fast 
and  the  entire  manipulator  configuration  changes  significantly  during  a 
single  cycle.   For  productivity,  all  joints  should  be  moved  simultaneously 


and  the  coupled  structure  of  the  manipulator  dynamics  makes  this  problem 
highly  nonlinear  and  difficult.  The  conventional  positioning  techniques 
currently  implemented  in  industrial  robots  are  no  longer  applicable. 

1  .1   The  Problem  of  Time  Optimal  Positioning 
with  State  Variable  Inequality  Constraints 

At  this  point  we  do  not  have  the  mathematical  tools  to  present  a 
mathematical  statement  of  the  major  problem  discussed  in  this  dissertation 
This  will  be  done  in  Section  4.1,  after  the  results  of  Chapter  II,  the 
manipulators  dynamic  model,  and  Chapter  III,  the  decoupling  transforma- 
tion have  been  obtained.   Nevertheless,  for  clarity  of  the  presentation, 
we  define  in  this  section  the  control  problem  in  general  terms ,  as 
follows . 

Given  a  N-DOF  manipulator  system  with  its   N  actuators  (DC  motors 
are  assumed),  where  all  the  relevant  parameters  of  the  load  and  manipu- 
lator are  known,  and  where  the  initial  and  final  manipulator  configura- 
tions are  given,  find  the  actuators  inputs  (voltages)  to  be  applied,  the 
controller  structure,  and  the  manipulator  state  trajectories  to  change 
the  manipulator  configuration  from  the  initial  to  the  final  one,  in 
minimum  time,  without  exceeding  the  preassigned  maximum  velocities, 
accelerations  and  jerks  for  all  joints.   This  will  be  called  the  time 
optimal  positioning  control  (TOPC)  with  state  variables  inequality 
constraints  (SVIC)  problem. 

It  should  be  clear  from  the  definitions  above  that  we  are  dealing 
with  slewing  mode  or  coarse  positioning,  of  point  to  point  tasks,  since 
only  in  this  case  can  the  path  be  freely  chosen  by  the  controller. 


# 
Constraints  of  this  type  were  shown  to  affect  the  higher  harmonic 
content  of  the  (driving)  command  signals  and  reduce  the  vibratory  transient 
response  in  the  flexible  system  [T-3],  [G-1  ] ,  [M-2]  and  [M-3] 


It  is  our  intention  to  solve  the  above  stated  problem  using  methods 
which  remain  applicable  to  a  larger  class  of  problems,  e.g.,  TOPC  problem 
with  actuator  losses  and  energy  consumption  constraints;  time  optimal 
point  to  manifold  control  and  so  on. 

1  .2   Background 
In  this  section  we  give  a  brief  survey  of  previous  works  which  are 
related  to  the  important  aspects  of  the  problem.   The  following  topics 
are  discussed:   mathematical  modelling  of  multilink  manipulators,  global 
linearization  and  decoupling  of  manipulatros  including  actuator  dynamics, 
control  and  optimal  control  of  robotic  manipulators  and  similar  mechanical 
structures . 

Various  approaches  were  used  in  deriving  the  eguations  of  motion  for 
the  type  of  mechanical  linkages  described  above.   The  pioneering  works  by 
Uicker  [U-1  ]  and  later  by  Bejczy  [B-1 ]  and  Hollerbach  [H-3]  were  based 
on  the  Lagrangian  mechanics  approach.   Orin  et  al .  [0-1]  gave  a  recursive 
algorithm  using  the  Newton-Euler  formulation.   Thomas  and  Tesar  [T-4] 
presented  an  alternate  derivation  using  the  kinematic  influence  coeffi- 
cients.  Horowitz  and  Tomizuka  [H-4]  derived  the  dynamic  model  using 
the  Gibbs-Appel  eguations  of  motion. 

In  all  of  those  derivations,  the  resulting  dynamical  model  (for 
control  purposes)  of  the  N-DOF,  serial  manipulator,  has  the  following 
form,  which  is  almost  exclusively  used  in  robotic  control  literature. 
(See  for  example  [T-4],  [H-4],  [B-3],  [D-1],  [G-5],  [K-4],  [C-2],  [Y-2], 
[V-2],  [P-3],  [G-4],  [S-2]  and  many  others .  ) 


t  =  j(e)  •  e  +  f  (e,  e_) 

where   9  is  the  vector  of  joint  coordinates,   T  is  the  vector  of 
applied  torques,   J(9)   is  the  inertia- matrix  anf   f_  is  the  vector  of 
velocity  related  and  gravity  terms  .   Notice  that  the  input  control  is 
assumed  to  be  the  torque  generated  by  the  actuators  as  seen  in  the  joints' 
shafts.   These  torques  may  be  generated  by  electrical,  hydraulic  or 
pneumatic  motors,  in  all  cases  however  the  torques  cannot  be  isntan- 
taneously  assigned  thus  the  above  model  is  an  approximation. 

The  N-DOF  manipulator  may  be  assumed  to  be  affected  substantially 
by  the  dynamics  of  the  N  actuators  located  at  its  joints.  The 
actuator's  dynamics  must  be  accounted  for  if  an  optimal  trajectory  con- 
trol is  to  be  executed.  This  is  especially  the  case  when  a  fast  motion 
is  desirable,  where  the  actuators  natural  frequencies  are  significant 
and,  thus,  the  driving  torques  can  no  longer  be  assumed  instantaneously 
controllable. 

Very  few  works  in  manipulator  control  were  found  to  include  a 
dynamic  model  for  the  manipulators'  drives.   Paul  [P-1 ]  assumes  that  the 
manipulator  driving  torques  are  instantaneously  controllable,  but  he 
utilizes  approximation  curves  to  account  for  the  loading  effects  and 
friction  of  the  actuators.   Cvetkovic  and  Vukobratovic  [C-2]  and 
Vukobratovic  and  Kircanski  [V-2]  assume  a  linear  time  invariant  third 
order  model  for  electric  motors  which  are  the  manipulators'  drives. 
As  shown  inChapter  II  the  dynamic  model  for  the  manipulator/ 
actuators  system  is  a  multi-input,  multi-output,  highly  coupled,  non- 
linear, and  high  order  (18  state  variables  for  six  DOF)  set  of 
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differential  equations.  This  made  any  attempt  to  derive  analytical  con- 
trol laws  for  even  the  simplest  tasks  impossible.  Moreover,  even  on- 
line numerical  integration  techniques,  which  had  to  be  repeated  for  each 
control  cycle,  due  to  system  and  load  variations,  were  shown  to  be  an 
excessive  computational  burden  and  to  require  a  huge  amount  of  memory. 
Thus,  the  need  for  model  simplification  techniques  such  as  decoupling 
and  linearization  became  essential  for  fast  and  efficient  control  of 
robotic  manipulators. 

Various  linearization  methods  were  used  by  many  authors  in  the 
control  of  robotic  manipulators.   Kahn  [K-1]  derives  a  linear  model  for 
a  three  DOF  manipulator  for  finding  a  time  suboptimal  controller,  by 
deletion  of  all  second  and  higher  order  dynamics  in  the  full  model.   He 
found,  however,  that  an  average  value  of  the  velocity  related  (nonlinear) 
terms  had  to  be  added  to  the  model  in  order  to  guarantee  suboptimality . 
This  means  that  some  apriori  knowledge  of  the  state  trajectory  of  the 
system  had  to  be  assumed . 

Other  linearization  techniques  (see  Pieper  [P-4],  Whitney  [W-1], 
Paul  [P-1],  Vukobratovic  [V-2],  Yuan  [Y-2],  Takegaki  [T-1]  and  many 
others)  assume  that  the  nominal  trajectories  of  the  system  are  given  in 
advance.   The  dynamic  model  is  linearized  about  those  trajectories, 
yielding  independent  joint  reference  commands  for  each  actuator.   In 
practice,  however,  the  manipulator  system,  due  to  the  variations  of 
real  working  conditions,  does  not  follow  the  nominal  trajectories. 
This  degrades  the  control  performance  and  may  even  induce  unstable 
responses.   To  overcome  this  problem  Paul  [P-1]  utilized  precomputed 
compensations  of  nonlinear  effects  as  additional  inputs.   Vukobratovic 


[V-2]  designed  asymptotic  regulator,  feedback  controllers,  for  each 
actuator,  to  guarantee  stability  about  the  nominal  paths,  and  Yuan  [Y-3] 
suggested  a  feed  forward  compensator  to  improve  the  performance  of  the 
linear  independent  joint  controllers . 

Decoupling  of  the  manipulator /actuators  system  to  a  set  of  lower 
order  subsystems,  was  also  attempted  by  various  authors  (see  for  example 
Hemami  [H-2],  Freund  [F-2],  Guez  [G-4]  and  Horowitz  [H-4])  as  a  method  of 
simplifying  the  manipulator  control  tasks  . 

The  literature  on  decoupling  of  nonlinear  systems  is  rich  but  not  well 
organized.   Porter  [P-5],  found  sufficient  conditions  for  the  existence 
of  a  decoupling  state  feedback  transformation.   Later  Majumdar  and 
Choudhury  [M-1  ]  developed  necessary  and  sufficient  conditions  for  decoupl- 
ing and  diagonalization  by  state  feedback  for  nonlinear  system,  which  are 
linear  in  control  input.   Freund  [F-3]  extended  the  results  obtained  by 
Porter,  to  the  case  of  non -purely  dynamic  systems.   (See  [K-2]  for  summary 
of  additional  results .  ) 

For  locomotion  and  robotic  manipulator  systems,  however,  the  use 
of  nonlinear  feedback  transformation  was  first  suggested  for  lineariza- 
tion, Hemami  [H-2],  and  later  for  decoupling,  Freund  [F-2],  Guez  [G-4], 
Horowitz  [H-4].   in  these  works,  however,  actuator  dynamics  is  not 
included  in  the  system  model  (this  reduces  the  model  order  by  33%)  and 
the  decoupling  transformation  is  derived  while  assuming  that  the  joints 
driving  forces  and  torgues  may  be  instantaneously  assigned.   Moreover, 
the  nonlinear  feedback  transformation  derived  in  [F-2]  reguires  the  on- 
line  inversion  of  a  NX N  state  dependent  matrix,  where   N  is  the  number 
of  DOF  of  the  manipulator.   This  is  an  extremely  difficult  task  for  the 
control  sampling  rate  which  is  generally  suitable  for  manipulative  tasks. 
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Also,  in  [H-4]  the  gravity  and  load  related  forces  were  deleted  in  the 
derivation  of  the  linearizing  and  decoupling  transformation. 

Other  decoupling  technigues,  such  as  the  linear  feedback  asymptotic 
regulator  by  Vukobratovic  [V-2]  and  the  open  loop,  feedforward  compen- 
sator by  Yuan  [Y-3]  were  suggested.   These  methods,  however,  assumed 
given  nominal  trajectories  about  which  the  model  is  linearized,  and 
therefore  represent  only  local  decoupling.   Another,  approximate 
decoupling  method  is  used  by  Koivo  and  Guo  [K-3]  where  the  first  three 
joints  of  a  six  DOF  manipulator  are  associated  with  the  translational 
motion  of  the  end  effector,  and  the  last  three  joints  with  its  orienta- 
tion.  The  coupling  terms  between  the  variables  of  these  two  groups  are 
neglected . 

As  will  be  shown  in  Chapters  II  and  III,  for  a  large  number  of  DOF, 
the  feedback  decoupling  may  involve  the  evaluation  of  thousands  of 
additions,  multiplications  and  trigonometric  operations.   On-line 
computations  of  those  may  therefore  take  many  minutes  for  a  single 
iteration  and  turn  out  to  be  impractical  for  real  time  control.   This, 
however,  is  not  the  only  possibility  for  the  feedback  decoupling  exe- 
cution.  Various  combinations  of  evaluations  of  analytical  expressions 
and  references  to  table  look-up  are  possible  as  a  method  of  computa- 
tional burden  reduction  in  each  cycle. 

In  1975  Albus  [A-1  ]  suggested  the  Cerebellar  Model  Articulation 
Controller  (CMAC)  as  a  new  approach  to  manipulator  control.   In  his 
work  he  describes  how  the  various  control  signals  for  a  multi-DOF 
manipulator  are  obtained  by  referring  to  a  table  (in  a  computer's 
memory)  rather  than  by  mathematical  solution  of  simultaneous  eguations . 
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CMAC  combines  input  commands  and  feedback  variables  into  an  input  vector 
which  is  used  to  address  a  memory  where  the  appropriate  output  variables 
are  stored.  In  [A-1]  and  later  in  [A-2]  Albus  describes  the  CMAC  memory- 
management  algorithm  which  makes  it  possible  to  store  the  necessary  data 
in  a  physical  memory  of  a  practical  size. 

In  fact,  as  was  shown  by  Raibert  [R-1],  by  appropriate  paramateriza- 
tion  of  the  equations  of  motion  for  the  manipulator,  divergent  procedures 
for  control  involving  analytical  expressions  and  table  look-up  can  be 
examined.   Each  procedure  represents  a  different  point  on  a  continuum 
characterized  by  the  indicator  P,  the  number  of  parametric  variables. 
As  P  increases  storage  requirements  increase,  but  computational  com- 
plexity decreases.   As  shown  by  Raibert  [R-1],  for  a  N-DOF  manipulator, 
with  3N  variables  (namely,  the  joint  positions  (N),  velocities  (N) 
and  accelerations  (N)),  of  which   P  are  held  constants  as  parameters 
whereas  the  other  3N-P  variables  are  represented  by  R  resolution  cells 

(3i>  P 

each,  about  N      arithmetic  operations  and   R   storage  cells  are 
required  for  evaluation  of  the  equations  of  motion. 

A  variety  of  other  control  structures  have  also  been  developed. 
Through  the  application  of  variable  structure  theory,  control  of  a 
simple  manipulator  was  accomplished  by  discontinuous  feedback  using 
sliding  modes  which  produced  a  somewhat  undesirable  discontinuous  con- 
trol signal.   In  [D-2] ,  a  model  reference  adaptive  controller  is  proposed 
which  forces  each  manipulator  joint  to  behave  as  an  underdamped,  second 
order  system,  with  the  actual  equations  of  motion  not  being  utilized 
in  the  design. 
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In  spite  of  the  importance  of  time  optimal  (fast)  motions  of  the 
manipulator  for  increased  productivity,  which  has  been  recognized  by 
many  (see  for  example  [P-3],  [K-1  ] ,  [L-4],  [Z-1],  [V-1],  [6-3],  [C-2] 
and  [L-3]),  very  few  works  are  known  to  be  performed  in  this  field. 
This  is,  of  course,  due  to  the  highly  nonlinear  and  highly  coupled  form 
of  the  differential  eguations  which  govern  the  system.   As  a  result,  very 
few  attempts  to  apply  optimal  control  methods  in  manipulator  control 
were  reported.   Kahn  [K-1]  presented  a  suboptimal  numerical  solution  to 
a  minimum  time  point  to  point  control  for  three  DOF  manipulator.   The 
dynamic  model  used  by  Kahn  did  not  include  the  actuators  dynamics,  and 
was  linearized  by  neglecting  second  and  higher  order  terms  in  the  egua- 
tions of  motion,  some  velocity  dependent  terms  however  were  approximated 
by  their  estimated  values.   In  spite  of  these  simplifying  assumptions 
the  resulting  control  law  was  still  dependent  on  the  boundary  conditions. 

Paul,  et  al.,  [P-3]  present  a  linear  programming  algorithm  for  finding 
the  near  minimum  time  path  of  the  manipulator  end  effector.   It  deals 
with  path  planning  by  designing  a  set  of  time  intervals  for  the  transi- 
tions among  a  seguence  of  preassigned  points  in  the  cartesian  space  for 
the  end  effector.   The  motion  between  each  pair  of  points  is  assumed 
along  a  straight  line  segment  with  constant  velocity.   The  total  traveling 
time  is  minimized  while  observing  velocity  and  acceleration  ineguality 
constraints.   The  manipulator  dynamics  are  not  included  and  since  the 
path  is  assigned  in  advance  it  does  not  solve  the  time  optimal  posi- 
tioning problem.   Later  this  method  was  extended  by  Lin  et  al .  [L-3] 
to  fitting  of  cubic  polynomials  with  velocity,  acceleration  and  jerk 
constraints  in  planning  a  minimum  traveling  time  trajectories  with 
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given  sequence  of  points.   Lynch  [L-4]  derives  a  sequential  mode  (one 
axis  at  a  time)  minimum  time  for  two  axis  manipulator.   In  a  sequential 
mode  coupling  between  the  various  axes  is  significantly  reduced,  the 
execution  time,  however,  is  much  larger  than  in  the  one  obtained  in 
simultaneous  motion  of  all  axes.   In  [G-2]  various  numerical  methods 
are  utilized  to  solve  a  three  DOF  mechanism  model  for  a  gymnast  per- 
forming a  kip-up  maneuver  on  a  horizontal  bar  in  minimum  time.   Again, 
no  actuator  dynamics  are  involved  in  this  case  and  the  optimal  control 
has  to  be  computed  for  each  new  set  of  boundary  conditions  . 

Other  works  in  the  area  of  optimal  control  of  robotic  manipulators 
deal  with  minimizations  of  quadratic  functionals  of  the  state  variables 
and  the  applied  torques  such  as  [T-5]  and  [Y-1  ] .   However,  only  second 
order  linear  models  are  used  and  the  actuators  dynamics  are  not  accounted 
for . 

A  related  problem,  the  optimal  control  of  container  cranes 
(minimizing  a  swing  of  the  load  as  it  moves  along  its   trajectory)  is 
described  in  [S-1],  again  the  actuators  are  not  modelled  and  the 
solution  is  nonanalytic ,  i.e.,  has  to  be  recomputed  for  each  task.   The 
only  reference  found  where  the  actuators  dynamics  are  accounted  for  in 
the  minimization  of  the  energy  dissipated  by  them  was  [C-2].   It  assumes, 
however,  no  interaction  between  the  manipulator  joints  and  is  therefore 
suboptimal . 

In  summary,  it  is  found  that  due  to  the  high  order,  highly  non- 
linear and  coupled  form  of  the  manipulators  equations  of  motion,  the 
direct  application  of  control  theory  to  the  problems  of  optimal  control 
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of  manipulator  is  impractical.   Therefore,  methods  of  global  lineari- 
zation and  decoupling  of  the  dynamic  model  are  essential  for  the 
successful  solution  of  the  optimal  control  problem. 

1 .3   Dissertation  Organization  and  Main  Results 
In  Chapter  II  the  dynamic  models  of  the  manipulator  and  actuators 
are  defined  and  augmented  into  a  single  state  space  model. 

In  Chapter  III  we  derive  the  feedback  decoupling  and  linearizing 
transformation  for  the  N-DOF  manipulator  system  including  its  actuator . 
This  transform  does  not  reguire  the  inversion  of  a  matrix  and  it 
accounts  for  all  torgue  terms  in  the  dynamic  model,  including  load  and 
gravity.   It  is  demonstrated  for  a  single  and  double  link  manipulator. 

In  Chapter  IV,  the  time  optimal  positioning  control  problem  is 
accurately  stated  and  solved  taking  into  account  various  task  configura- 
tions.  The  solution  is  analytic  and  need  not  be  repeated  for  each  task. 
Again,  it  is  simulated  for  a  single  and  double  link  manipulator  and 
then  compared  with  bang-bang  voltage  control . 

In  Chapter  V  the  approach  taken  by  our  work  is  shown  to  be 
appropriate  to  slewing  mode  positioning  via  a  simulation  of  a  guantized 
decoupling  transform,  indicating  a  low  sensitivity  of  the  output  motion 
to  this  type  of  error.   Additionally,  an  existence  theorem  is  proved 
regarding  the  asymptotic  stabilizer  for  a  manipulator  system  controlled 
by  imperfect  decoupling  feedback. 

The  dissertation  is  concluded  with  suggested  controller  structures, 
conclusions  and  topics  for  future  work,  which  are  described  in  Chapter  VI, 


CHAPTER  II 
DYNAMIC  MODELS  OF  THE  MULTILINK  MANIPULATOR  WITH  ACTUATORS 

In  this  chapter  the  dynamic  rigid  and  deterministic  model  for  the 
N-degrees  of  freedom  manipulator  and  its  actuators  is  selected.   Section 
2.1  describes  the  rigid  and  deterministic  dynamic  model  for  a  serial 
N-DOF  manipulator  and  defines  the  mathematical  properties  and  the 
physical  meaning  of  each  term  in  that  model.   Section  2.2  describes  a 
linear  third-order  time  invariant  model  for  the   N  permanent  magnet  DC 
motors  which  are  assumed  to  serve  as  the  manipulators'  actuators.   In 
Section  2.3  both  dynamic  models  are  combined  into  a  single  model,  for 
which  common  state  variable  and  differential  equations'  vectors  are 
defined. 

The  following  major  assumptions  are  made  in  this  chapter  and  will 
be  used  throughout  this  work. 

The  manipulator  is  an  open  loop  chain  (serial)  of   N  rigid  bodies . 
All  kinematic  and  dynamic  parameters  of  the  manipulator  and  its 
actuators  are  known  (given  constants),  thus,  a  deterministic  model 
is  assumed. 

The  manipulators'  joints  are  either  rotary  or  linear  (sliding). 
The  actuator's  dynamic  model  is  linear  and  time  invariant. 
The  external  loads  applied  to  the  manipulator  are  known  and  can  be 
expressed  as  smooth  (dif f erentiable )  functions  of  the  joints  dis- 
placements, velocities  and  time. 
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2.1   Manipulator  Dynamic  Model 
The  robot  manipulator  system  is  composed  of   N  rigid  links, 
connected  in  series  by  kinematic  rotary  or  linear  joints,  into  an  open 
loop  kinematic  chain.   (See  Figure  1  .)   As  mentioned  above  the  dynamic 
model  for  the  manipulator  system  is  given  in  Eguation  (2.1 .1 )  and  may 
be  described  as  follows: 


T  =  J  (9,  t)  to  +  f_(9,  »,  t) 


(2.1  .1  ) 


where 


f  (9,  to,  t)  =  £  (9,  to,  t)  + 


T  1 
(0   G  (0)  to 

T  2 
(o   G  (9)  to 


T  N 
(0   G  (9)  co 


(2.1  .2) 


and  where 


e  =  (e^  e  ,  . 


9  )    is  the  joints  linear  or  angular  posi- 


N 
tions  vector,  9eR  ; 


(o  =  (^  ,  to2> 


V     -  <V  e2> 


9  )    is  the  joints 
N 


linear  or  angular  velocity  vector,  toeR  ; 

...  •   T 

«  ■  iM,j ,  a^,  .  .  .  »„)    is  the  joints  linear  or  angular 

accelerations  vector,  toeR  ; 

T  =  (T.J  ,  Tjj  .  .  .  TN)    is  the  vector  of  generalized  driving 

forces,  (forces  or  torgues  acting  at  sliding  or  rotating 

joints  respectively)  TeRN: 
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J  (9,  t)   is  the  NXN,  "effective"  inertia,  symmetric  matrix  function 

of  the  joints'  coordinates  vector   9,  J(  ):  RN+1  -  RmN ; 
.£(£,  to,  t)   is  the  vector  of  gravity  and  external  loads  related 
torques/forces,  and  is  smooth  vectorial  function  of  the 
joints  coordinates  vector   9,  the  joints  rates,   to  and 
time   1(6,  (J,  t):  R2N+1  -  RN; 
G  (9),  i  =  1,  2,  .  .  .  N  are  the  NXN  symmetric  matrices  repre- 
senting the  velocity  dependent  torque  weighting  matrices  and 
are  functions  of  the  joint  coordinates  vector  9, 
GUey.n"-^      i-1.2.  .  .  .M. 
It  should  be  noted  that  the  components  of  the  matrices 
J(9,t)   and   G  (9)   for   i  =  1,  2,  .  .  .  N  are  products  of  trigonometric 
functions  (sines  and  cosines)  of  the  various  joints'  coordinates. 
Also,  since  the  external  loads  were  assumed  to  be  smooth  functions  of 
9,  to  and  the  gravity  and  load  related  torques  vector,   _£(9,  to,  t)   and 
J(9)  and   G  (9)   are  all  dif ferentiable  with  respect  to  all  their 
arquments.   This  will  be  exploited  later  in  Chapter  III  in  derivinq  the 
feedback  decouplinq  transformation. 

Notice  also  that  the  so-called  "effective  inertia"  matrix,   J(9,  t) 
at  any  given  time,   t,   is  positive  definite  for  all   9eRN  and  all 
possible  combinations  of  sliding  and  rotary  joints.   This  property  of 
J(9,  t)   is  proved  in  [H-4],  but  is  not  surprising  since  the  total 


In  general   £   is  a  function  of  the  external  loads  (torques  and 
forces)  and  qravity.   Since  these  loads  were  assumed  to  be  known  functions 
of   9,  to  and   t,  it  is  always  possible  to  obtain  the  form  l(Q,    to,  t) 
by  substitution.  — 
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kinetic  energy  of  the  N-DOF  manipulator,  which  is  always  a  positive 

T 
quantity,  is  given  by  ^w   •  J (9,  t)  w,  and  is  zero  only  for  zero  angular 

velocity,  i.e.,   J  (9,  t)   is  positive  definite  by  definition.   (See  [T-4] . ) 

The  various  components  of  the  above  described  matrices  and  vectors 

possess  an  important  physical  interpretation.   The  i-th  diagonal  entry 

of   J(9_,  t),  Jii(9.  t),   represents  the  effective  inertia  (including 

the  load,  which  may  vary  in  time)  at  joint   i;  the  off -diagonal  entry 

of   J (6,  t),  Jik(9>  t),   represents  the  coupling  inertia  between  joints 

i   and  k .   Similarly,   G^Q)   represents  the  centripetal  force  at 

joint  i  due  to  velocity  at  joint  k ;   G1  (9)   represents  the  force 

at  joint   i  due  to  velocities  at  joints   j   and  k ;   and   £. (0,  w,  t) 

represents  the  torgue  developed  at  joint  i  due  to  gravity  and  external 

loads . 


2.2   Actuators  Dynamic  Model 
The  N-DOF  manipulator  is  assumed  to  be  driven  by   N  actuators 
located  at  its  joints . 

Throughout  this  work,  we  assume  (using  an  approach  similar  to  the 
one  taken  in  [C-2]  and  [V-2] )  that  the  actuators  are  permanent  magnet, 
DC  motors,  with  armature  current  control.    The  i-th  actuator  is  acting 


Linear  models  of  other  types  of  actuators  may  be  used  without 
loss  of  generality  of  the  work  that  follows. 
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on  the  i-th  joint  through  a  gear  reduction  box  and  may  be  described  as 
follows . 


A.9.    +  B.oj.    +  J. io.    +  T.    =  r      K        I 
11  li  li  i  l      T.       i 


L.  I.    +   R.  I.    +  r.  K        u>.    =   V 
li  li  l    V.       i  i 

l 


i    -   1  ,    2,    .    .    .    N  (2.2.1 


where 


V.  is  the  i-th  motor  input  voltage; 

TV  is  the  i-th  joint  load  torgue  as  given  in  (2.1.1); 

I.  is  the  i-th  actuator's  armature  current; 

r.  is  the  i-th  gear-box  reduction  ratio  (r.  >  1  ); 

J^   is  the  i-th  actuator  rotor  inertia  referred  to  the  output 

2 
shaft,  (J^  =  Jm   .  r  ).  (J    is  the  moment  of  inertia  of 

i  i 

the  i-th  motor.) 

Bj,  is  the  i-th  actuator's  viscous  friction  coefficient; 

Ai  is  the  i-th  actuator's  compliance  coefficient; 

R^  is  the  i-th  armature  ohmic  resistance; 

L.  is  the  i-th  armature  inductance; 

KT   is  tne  i-th  actuator  torgue  constant; 

i 


and 


1^   is  the  i-th  actuator  back  e.m.f .  constant, 
i 

In  addition  to  the  above  given  constant  parameters,  the  rated  torgue 
and  speed,  the  maximum  acceleration  and  the  maximum  speed  are  given. 
Notice  that  although  all  the  motors'  parameters  were  assumed  known 
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(given),  they  are  in  fact,  the  result  of  a  design  procedure  which  selects 

the  appropriate  actuators  for  the  given  manipulator  and  the  desired 

volume  of  tasks . 

The   N  eguations  (2.2.1  )  may  be  rewritten  in  a  vector  form  as 
follows: 

J  io  +  T  =  C  I  -  A9  -  B  u 

(2 

.2.2) 

I  «  P.  to  +  F  I    +   L~1  V 

where 

T  is  the  vector  of  joint  loading  torques  as  defined  in  Section 

2.1  ; 

V  is  the  vector  of  input  voltages  with  components   V. ; 

I  is  the  vector  of  armature  currents  with  components   I  : 

i 

J  is  a  diagonal  matrix  with   J. .  =  J  >  0   for   i  -  1   2 

n     i                ,   ,  .  .  . 

N, 

and  is  therefore  positive  definite; 

C  is  a  diagonal  matrix  with   C. .  =  r   K   >  0   for 

n    l   T. 

l 

i  =  1 >  2,  .  .  .  N,  and  is  therefore  positive  definite; 

A  is  a  diagonal  matrix  with   A. .  =  A.  >  0   for   i  =  1  ,  2,  . 

N; 

B  is  a  diagonal  matrix  with   B. .  =  B.  >  0   for   i  =  1 ,  2. 

n    l                ,   ,  .  .  . 

N; 

L  is  a  diagonal  matrix  with   L. .  =L.  >  0   for  i  =  1,  2,       N 

and  is  therefore  positive  definite; 

ri  V 

F1   is  a  diagonal  matrix  with   F    = <  0   for 

1  •  t              Li , 

n       i 

1  =  1>  2,  •  •  •  N  and  is  therefore  negative  definite; 

» 

» 
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R. 

F2   is  a  diagonal  matrix  with   F    = <  0   for  i  =  1 ,  2 , 

n      i 
and  is  therefore  negative  definite; 

and  where  a  diagonal  matrix   M  is  defined  as  a  matrix   M  with 
mij  =  °   for   Mi      i,  j  =  1,  2,  .  .  .  n. 

Eguation  (2.2.2)  defines  a  combined  dynamic  model  for  the  N 
actuators  used.   In  the  following  section  we  use  Equations  (2.1  .1  ), 
(2.1  .2)  and  (2.2.2)  to  obtain  an  augmented  model  for  the  N-DOF  manipu- 
lator with  its  actuators  . 


2.3   Augmented  Model 

In  this  section  we  combine  the  equations  of  motion  for  the  N-DOF 
manipulator  with  the  dynamic  model  of  its  actuators.   This  augmented 
model  is  reguired  for  a  clear  problem  formulation  and  efficient  deriva- 
tion of  an  analytical  control  law. 

Using  state  space  notation,  let  the  state  variables  for  the 
augmented  system  be  the  joint  coordinates,  the  joint  velocities  and  the 
actuators'  armature  currents.   That  is,  define 


*i  -£ 


X   =  u 

-2   -  (2.3.1) 


*3  =1 


and 


x  =  (V  x2,  x3)T. 
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In  terms  of  X (  ,  X_2   and  X^  Equations  (2.1.1),  (2.1.2)  and  (2.2.2) 
may  be  written  as 


J^,  t)  X2  +  fCX^,  X2,  t)  =  T 


(2.3.2) 


fOL,,  x2,  t)  .m1(  x2,  t)  + 


T   1 
X_2  G  (X}   )  X2 


xJg2^)^ 


T   N 
X2  G  %   }  *2 


(2.3.3) 


JX  2  +  T  =  CX  3  -  AX  1  -  BX 


5.3  =  F1*2  +  F2-3  +  L  - 


Combining  (2.3.4)  and  (2.3.2)  we  obtain 


(2.3.4) 


tJ  +  Jt*,  ,  t)]  X2  +  FO^  ,  X2,  t)  =  CX3  -  ^  -  BX 


-2 


or  in  a  state  variable  form 


*1  =  *2 


X2  =  g(X,  t) 


(2.3.5) 


X3  =  FlX2  ♦  F2X3  +  L"1V 
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where 


g(X,  t)  =  J(Xt,  t)  1  [CX3  -  f^,  X2,  t)  -  K^    -   BX2]       (2.3.6) 
and 

J(X_1  ,  t)  =  j(x  ,  t)  +  J  (2.3.7) 

Thus,  Equations  (2.3.5),  (2.3.6)  and  (2.3.7)  represent  the  complete 
state  space  model  for  the  N-DOF  manipulator  with  its  actuator. 

It  should  be  noted  that  since   J  and   J(X  ,  t)  are  positive 
definite,  so  is   J(X_1  ,  t)   and  is  therefore  nonsingular  and  the  state 
space  model  exists  for  any  configuration   (V  9eRN)   of  the  manipulator. 
See  Figure  2  for  a  block  diagram  of  the  augmented  system. 
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Figure  2.   Block  Diagram  of  Augmented  System 
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CHAPTER  III 
A  FEEDBACK  LINEARIZING  AND  DECOUPLING  TRANSFORMATION  (FLDT) 

In  the  previous  chapter  it  was  shown  that  the  dynamic  model  for 
the  manipulator /actuators  system  is  a  multi-input,  multi-output,  highly 
coupled,  nonlinear,  and  high  order  (18  state  variables  for  six  DOF) 
set  of  differential  eguations .   This  made  any  attempt  to  derive  analytical 
control  laws  for  even  the  simplest  tasks  impossible.   Moreover,  even  on- 
line numerical  integration  techniques,  which  had  to  be  repeated  for 
each  control  cycle,  due  to  system  and  load  variations,  were  shown  to  be 
an  excessive  computational  burden,  (see  Section  1  .2)  and  to  require  a 
huge  amount  of  memory.   Thus,  the  need  for  model  simplification  techniques 
such  as  decoupling  and  linearization  is  essential  for  fast  and  efficient 
control  of  robotic  manipulators  . 

In  this  chapter,  a  nonlinear  state  feedback  transformation  is 
introduced  to  the  full  manipulator /actuator  system  via  the  actuators' 
input  controls.   This  transformation  takes  into  account  the  actuator 
dynamics,  gravity  and  load  related  terms,  requires  no  matrix  inversion, 
and  provides  global  decoupling  of  the  N-DOF  system  into   N  linear 
subsystems.   Each  decoupled  subsystem  represents  one  DOF  of  motion 
(link)  of  the  manipulator  with  the  position,  rate  and  acceleration  of 
the  DOF  as  its  state  variables  which  preserves  physical  insight  to 
the  control  problem  and  the  link's  :erk  as  its  input.   The  dynamics 
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of  each  DOF  of  the  manipulator  may  be  arbitrarily  (and  independently 
of  the  other  DOFs)  assigned  by  a  proper  choice  of  a  linear  feedback. 
Furthermore,  the  parameters  of  the  transformation,  do  not  need  to  be 
readjusted  for  different  tasks  (paths)  as  is  the  case  in  linear  control 
schemes  and  all  axes  may  have  the  same  dynamics  for  all  configurations 
and  parts  of  the  manipulator's  state  space. 

We  commence  in  Section  3-1  by  the  derivation  of  the  feedback 
linearizing  and  decoupling  transformation  (FLDT)  based  on  the  dynamic 
model  presented  in  Chapter  II.   Sections  3-2  and  3.3  demonstrate  the 
application  of  the  FLDT  for  single  and  double  link  manipulators. 

3.1   The  Decoupling  and  Linearizing  Transformation 
In  this  section  a  nonlinear  state  transformation  is  derived  such 
that  in  terms  of  the  new  state  variables  the  manipulator/actuator 
system  is  linear  and  decoupled  to  N  subsystems.   Each  subsystem  repre- 
sents a  single  link  (DOF)  in  a  form  of  a  triple  integrator  dynamics. 

The  equations  of  motion  of  the  augmented  system  were  given  in 
Section  2.3  and  are  restated  here  for  convenience. 


*1  -h 

12  =  SSh   fc)  (3.1.1) 

13  =  F^2   +  F2-3  +  L_1  - 

where 
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g(x,  t)  =  J-1^,   t)    [cx3  -  AX j    -  bx 2  -  wo^,  x2) 


»<*,.  x2)  =  txJ  g1  ol,  )  fef  xJg2^)^,   ....  (3'1-2 

T      N  T 

X2    G    (X1  )    X2]L 

and  where  X  =  (^  ,  x_2 ,  X_3  )T  with  X 1  ,  X_2   and  X_3   as  defined  in 
Equation  (2.3.1 ) . 

In  order  to  obtain  a  decoupled  set  of   N  triple  integrator  sub- 
systems, let  the  following  nonlinear  state  transformation  be  introduced: 


(3.1  .3) 


(3.1  .4) 


Y  =  <P(X,  t) 
where 

i(X,  t)  [x^  X2,  g(X,  t)]T 
and  where 

Y*R3N. 
Equivalents,  Equation  (3.1.3)  may  be  written  as  follows: 

12  =  -2  =  2-1  (3.1.5) 

13  ■  2.(x.'  t)  =  x2 

i.e.,   Yu,  Y2i   and   Y3i   represent  the  position,  rate  and  acceleration 
of  the  i-th  joint,  respectively. 

The  equations  of  motion  of  the  system,  in  terms  of  the  new  state #  vector, 
Y,  are  found  by  applying  Equations  (3.1.1)  and  (3.1.2)  to  (3.1.5).   We 
obtain: 


#    . 
Y  is  a  state  variables  vector  since  the  mappinq  q (X )   is  invertihU 
with  respect  to  X g .   (j   (^  )   and   <.-*   exist).  mvertible 
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2,  -.£,-■  *a  -  &  . 


or 


v      -   v 

-1         -2    *  (3.1  .6) 

Similarly,  for   Y 


I2  =  -2  =  2.(-'  fc)  =  I3  ' 


or 


Y   =  Y 

-2   -3  *  (3.1  .7) 

Finally,    for      Y       we  obtain 

Y3    =  g(X,    t)    =  -A_  [J"1  (X^    t)]     [tX3    -    AX 1     -    BX2    -   *<£,  ,    X ■  ) 
~  l%>    *2>    t>]    +  J~1  CX^,    t)    [d3    -  M^    -  bL   -   W(X) 

-I«^    t)]  (3.1.8) 

Equation  (3.1.8)  is  the  only  state  variable  equation  which  involves 
the  input  vector,   V  (through  X g )  .   Thus,  the  state  transformation  may 
be  performed  via  state  feedback  by  a  proper  choice  of  the  input  function. 
Now,  in  order  to  obtain  a  triple  integrator  form,  we  must  have 

• 

-3  =  -  (3.1.9) 

where   2  is  the  new  control  input  vector,  ZeRN.   Thus  Equations 
(3.1.8)  and  (3.1.9)  define  the  state  feedback  as  follows: 
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Z  =    Y3    =  -TT  [J  1  (X    •    t)]     [«    -   AX      -    BX2    -   W(X    ,    X2) 
-  £0^,   x2>   t)]    +     J"   (X,,    t)    [qL  -  AX,    -  JB|L   -W(X) 


-   *<X,    t)]     , 


using   Equations    (3.1.1)    and    (3.1.2)    again  yield 


z  =  —  [j"  (Xjj,  t)]  Cqjg  -  iKj,  -  BKg  -  woc^  x2) 


-1  -1 

-  £CX.,i    X_2,    t)]    +   J       (X,.    t)     [CFX      +    CF2-3    +CL      - 


-   AX      -    B  g(X,    t)    -   W(X)    -   £(Xj    t)] 


(3.1  .10) 


The  feedback  transform     V(  X,Z,    t)      is   obtained   from   Equation    (3.1.10) 
as    follows: 


1  r\  1 

v(x,   z,  t)  =  lc~  J(x1 ,  t)    {z  -  -|r-[J"   (X,,  t)l    [qc_3  -  ax 


-BX2    -   JKX.j,    X_2)    -  £0^,    X2,    t)]}    +   LC   1[AX_2 


+    B  g(X,    t)    +    W(X)    +   £(X,    t)]     -L[F1X2    +    P  X     ; 


(3.1  .11  ) 


where 


d     r    -1  , 

"dT[J    (2L,»  t>] 


n    otj'1^  t>]  ,         a[J"1(^'t)] 
z       ^r^ x2j  +  at 


j-1 


ax. 


id 


£(x,  t)  = 


QL%>  ?_2,  t) 


V 


9£(X1 ,    X2 ,    t ) 


g(x,   t) 


at 


W(X) 


< 

gT(X)    G1  (x.,  )  X2 


gT(x)  G2^  )  x2 
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i 

V 


/• 


T  N 

g  (x )  g  (x^  )  x2 


5.2  °1  (~1  '  9-(-) 


X_2    G2  (X    )   g  (X  ) 


T      N 
X2    G    (X^  )   g  (X  ) 


xT 

-2 


xT 

-2 


N  6G1 (X    ) 

:=1  1 3         J 


N        3G    (X     ) 

•\       ax,  .      x2j 


*2 


*2 


T 
*2 


N        3GN(X    ) 


*2 


(3.1  .12) 


and  where  we  define 
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8a  (b) 
3b~~ 


9b/ 


aa2 


is  the  Jacobian  matrix 
=  of  the  MX1  vector  a  with  = 
respect  to  the  NX1  vector 
b. 


3a 
m 

ob? 


ab2' 


aa2 

ab2« 


6a 


m 


»,' 


3a, 

3b. 


3a, 
3b" 


3a 


m 


3b. 


and 


AM(b) 
o  — 


3b 


Matrix  of  the  partial 
derivatives-  of  a  matrix 
MNXM  with  respect  to  a 
scalar  argument   b  . 


3m 


1  1 


3b 


dm 
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3b 


3m 
3b 


N1 


3m 


12 


3b.  ' 

3 


3m 
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3b 


3m 
3b 


N2 


3  m 


1M 


3b 


3m 


2M 


3b 


3m. 


NM 


ab 


s 
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Clearly,  in  the  general  case,  the  feedback  transform,  V(X,  Z,  t) 
as  defined  in  Equations  (3.1  .11  )  and  (3.1  .12),  is  a  function  of  all 
state  variables,  including  X  ,   the  armature  currents.   In  the  new 

tate  space,  however,   ( Y) ,  the  armature  currents  are  not  directly 
represented.   It  is  therefore  desirable  to  represent  the  feedback 
linearizing/decoupling  transformation  (FLDT)  in  terms  of   Z,  t  and 
Y  only.   That  is  measurements  of  the  joints'  positions,  rates  and 
accelerations  only  are  required,  and  not  the  currents.   This  is  done  as 
follows.   From  Equations  (3.1.2)  and  (3.1.5) 

ex 3  =  ax 1  +  bx2  +  wq^  ,  x2)_  +  ux_^ ,  x_2,  t)  +  jcj, ,  t)  g(x,  t) 

or 

CX_3  =  AY^  +  BY2  +  WCY,,  Y2)  +  £<Y,,  Yg,  t )  +   J(X,  ,  t)  Y.    (3.1.13) 

Notice  that      C     and      F2      are   both    NXN  diagonal  matrices   and  terefore 
commute    (i.e.,      CF2   =   F  C),    thus 

CF2-3    =    F2[A-1    +   B^2    +  -(-T    ty    +  --1'    V    t} 

+   J(Y,,    t)    Y33  (3.-,  .14) 

Also  recall  that  for  a  nonsingular,  square  matrix,   A(t),   the 
following  relation  holds: 

-5r[A_1(t)]  =  -  A"1(t)  -iL  [A(t)]  A"1(t)  (3.1.15) 

By   applying    Equations    (3.1.15),     (3.1.14)    and    (3.1.13)    in    (3.1.11) 

we  obtain   after   simple  manipulations   the    FLDT  as   a  function  of   the   new 

state   vector      Y     and   time,    t,    as   follows: 
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V(Y,    Z,    t)    =    LC_        {J(Yf   t)    Z   +   W(Y)    +  £(Y,    t)    -    F2W(Y    ,    Y. ) 
-    F2-(-1  '    ^2'    fc)    "    F2A-1    +    [A   _    CFr  F2B]    -2 


+    [B   -    F2    J(Y1,    t)    +   JCY.J,    Y2,    t)]    Y3} 


(3.1  .16) 


where 


J'% » 

Y2,    t)    . 

N 
j=1 

8J(Y-  »    t) 

Y2j    + 

aj(Y  ,  t) 

I      ^3        J 

at 

W(Y)    = 


ij  g1  (Ig,  )  I2 


T      2 

I3  G  <*, )  I2 


T      N 
Y3    G    (Xl  )    Y2 


T      1 
Y2    G    (Y    )    Y3 


T      2 
*2   G    (*1  >   *3 


T      N 

4  G  (Ii '  & 


4 


ll 


N     3  G1  *  Y    ) 
3=1         1  3  J 


N  3G    (Y    ) 

I     —  Y 

*  3Y.   .              2j 

3-1  1 3               J 


*2 


4 


L 


-    N 
N     6G    (Y    ) 

j=1    dT^.  Z2j 


dZ ( Y    ,    Y.  ,    t )    , 

m.  «.  — L^ — 4  + 


^i 


•3 


I3  + 


9i(Il  •  12>  t} 
at 


(3.1  .17) 
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Equations  (3.1.16)  and  (3.1.17)  define  the  FLDT.   Notice  that  in  view 
of  the  assumptions  in  Chapter  II,   C~   exists  and  is  readily  available 
(since   C  is  diagonal,  see  (2.2.2)),  and   £_(Y  ,  Y  ,  t)   was  assumed 
differentiate ,  thus   V(Y,  Z,  t)   in  (3.1.16)  exists  for  all   t,  and 
all  points  in  the  state  space   YeR   .   Moreover,  the  FLDT  employs  only 
immediately  available  system  matrices  such  as   J(Y ,  t),  W(Y  ,  Y  )   and 
their  time  derivatives;  no  matrix  inversion  is  required. 

In  the  case  that  a  stationary  (time  invariant)  external  load  is 
applied,  the  inertia  matrix  J  and  the  load  torque  vector   £  become 
time  invariant.   This  simplifies  the  evaluation  of  the  FLDT  which 
becomes  time  invariant  as  well . 

To  summarize,  when  the  FLDT  as  given  in  Equations  (3.1  .16)  and 
(3.1.17)  is  applied  to  the  system  (Equation  (3.1.1))  we  obtain  the 
following  system  (which  from  now  on  will  be  denoted  as  the  equivalent 
open  loop  system,   EOS) 


Y1  = 

Y2 

• 

*2    = 

h 

• 

Y3    = 

Z. 

In   a  scalar  form   Equation    (3.1.18)  may  t 

• 

Y1i   = 
Y2i    ' 

1   Y2i 

1   Y3i 

hi    - 

Zi    ,         i    =   1  ,    2 ,     .    .    .    N    ; 

(3.1  .18) 


(3.1  .19) 
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which,  as  required,  represents  N  decoupled  triple  integrator  sub- 
systems with  the  i-th  input,   Z. ,  representing  the  i-th  joint's  jerk. 

Figure  3  describes  the  manipulator/actuators  system,  the  FLDT  and 
the  new  equivalent,  open  loop  system  EOS. 

Notice  that  in  Equation  (3. 1.18)  the  EOS  is  unstable  since  its 

=  0.  Never- 
theless, the  EOS  is  reachable  since  the  3NX3N  matrix  M,   where 

r 


characteristic  polynomial  may  easily  be  shown  to  be  S^ 


M 


-\ 


0 

0 

\ 

> 

0 


N 


0 
0 


0 


N 


0   0 
0   0 


N 


0   0 
0   0 


V 


has  full  rank  (3N)  and,  therefore,  according  to  the  pole  placement 
theorem  the  EOS  may  have  arbitrarily  assigned  dynamics  by  a  proper 
choice  of  a  linear  time  invariant  feedback.   In  fact,  due  to  the  de- 
coupled structure  of  the  EOS,  and  as  can  be  seen  from  Equation  (3.1.19), 
similar  arguments  hold  for  each  i-th  subsystem  which  is  stabilizable 
and  by  a  proper  choice  of  a. .,   j  =  1,  2,  3  in  (3.1.20) 


Zi  ■  aliY1i  +  ai2Y2i  +  ai3Y3i 


(3.1.20) 


any  characteristic  polynomial  may  be  selected  for  each  i-th  subsystem 
as  well  as  the  EOS. 
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3.2   FLDT  for  a  Single  Link  Manipulator  (Example  1) 
In  this  section  we  apply  the  FLDT  developed  in  Section  3.1  to  a 

single  DOF  case,  i.e.,  a  DC  motor  with  a  link  attached  to  its  shaft. 

(See  Figure  4.)   This  example  will,  of  course,  demonstrate  linearization 

only  since  coupling  is  irrelevant  in  a  single  DOF  case. 

The  eguations  of  motion  for  the  system  are  derived  in  Appendix  I 

and  are  given  by  (see  Equation  (A1-4)) 

X#1  =  X2 

X2  =  J-1[CX3  -  £(Xl)]  (3.2.1  ) 

*3  =f1X2  +f2X3  +  L_1V 

where,  as  in  Chapter  II 

X1   is  the  link's  angle  measured  with  respect  to  the  vertical 
(downward )  axis ; 

X2   is  the  link's  angular  velocity; 

X   is  the  armature  current; 

V   is  the  applied  armature  voltage; 

J   is  the  total  moment  of  inertia  reflected  to  the  motor 
output  shaft   (J  =  J  r2  +  md2 ) ; 

m,  d   are  the  link's  mass  and  length  respectively; 
-    g   is  the  gravity  acceleration; 

KT'  KV'  R'  L  are  the  motor's  torque  constant,  back  emf,  armature 

resistance  and  armature  inductance  respectively; 
r   is  the  speed  reduction  ratio  of  the  gear  box; 
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Gravity- 


Figure  4.   A  Single  Link  Manipulator 
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and  where 

C  =  r   KT;      £(X1 )    =  mgd   sin(X1 )|   f,  ■  "  2j      f,    .          * 

•                                                                                                                       ^-                          Li                                                                         . 

L 

' 

Following  the  notations   of    Section   3.1    let 

A 

Y1    =X1 

A 

Y2    =  X2                                                                                                                                       (3.2.2) 

Y3    =   J      [CX3    -   £(X    )] 

and  evaluate   the    FLDT  as  given   in   Eguation    (3.1.16)    to  this   case,    noting 

that     W,    A,    B     and      J     vanish, 

V(Y,    Z)    =   LC"      [JZ   +  mgd    Y2   cos^  )    -  f     mgd   sin(Y    ) 

~   a\?l    ~   f2    J  Y3]                                                                                     (3.2.3) 

Applying  the    FLDT  given   in   Eguation    (3.2.3)    to   the    System    (3.2.1) 

we  obtain  the   EOS  as   follows: 

. 

Y      =    Y 
1          X2 

• 

Y2    ~    Y3                                                                                                                                       (3.2.4) 

and 

Y3    ="dV[J"1(CT3    -    *(V]    *    J_1[CX3    -    *Xj>l    =    J~1[Cf1Y2 

+  f2JY3    +   JZ   +   *(Yt)    -    Cf1Y2    -   f2JY3    -    i(Yl)]    .   J-1[JZ]            (3.2.5) 

■ 

. 
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l.  e. , 


Y3  -  Z 


Equations  (3.2.4)  and  (3.2.5)  describe  (as  expected)  the  EOS  as  a 
triple  integrator  system,  with   Z(t)   as  the  new  input,  which  physically 
represents  the  angular  jerk  of  link. 

It  is  interesting  to  notice  that,  although  the  EOS  state  variables 
Y1  ,  Y2   and   Y3   (i.e.,  the  angular  position,  rate  and  acceleration  of 
link)  form  a  linear  dynamical  system  (a  triple  integrator);  the  arma- 
ture current,   X3>   which  is  not  an  EOS  state  variable  is  nonlinearly 
dependent  on   5^   and   Y3   as  follows  (see  Equation  (3.2.2): 

X3  =  C"  tJY3  +  mgd  Bin(V]  (3.2.6) 

For  this  purpose  the  step  response  of  the  EOS,  and  the  corresponding 
FLDT  (armature  applied  voltage)  and  armature  current  variation  were 
computed  in  Experiment  1  and  shown  in  Figure  5  as  follows. 
Experiment  1  .   A  FORTRAN  program  (EXP1  )  which  simulates  the  system 
given  in  Equation   (3.2.1)  was  written  (its  listings  are  given  in 
Appendix  III).   The  input  voltage  was  evaluated  via  the  FLDT  (3.2.3) 
for  the  DC  motor  PMI  MC  19P  with  a  jerk  step  of  40  rad/s3  and  zero 
initial  conditions  and  the  following  parameter  values: 

m  =  10,  g  =  10,  d  =  1  ,  R  =  0.6,  L  =  .0015,  r  =  10,  K  =  .25,  K  =  .25. 

Figure  5  describes  the  step  responses  of  the  armature  voltage  (V) 
and  current  (I)  and  the  links  angular  velocity,  acceleration,  and  position 
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Figure  5.   Step-Response  of  the  Equivalent  Open-Loop  System 
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respectively.   It  demonstrates  that  the  angular  velocity,  acceleration 
and  position  step  responses  are,  in  fact,  those  of  a  triple  integrator 
system,  whereas,   V  and   I  illustrate  the  expected  nonlinear  behavior 
due  to  gravity. 

3.3   FLDT  for  a  Double  Link  Manipulator  (Example  2) 
In  this  section  we  apply  the  FLDT  derived  in  Section  3.1  to  a 
double  link  planar  manipulator  driven  by  two  DC  motors .   This  example 
demonstrates  linearizstion  as  well  as  decoupling  of  the  general  N-DOF 
manipulator  system  since  it  contains  all  major  dynamical  components 
such  as  gravity,  Coriolis  and  centripetal  terms. 

Figure  6  describes  the  double  link  system  configuration.   The 
eguations  of  motion  for  this  system  are  derived  in  Appendix  II  using  the 
Lagrange  method.   We  reformulate  them  here  using  the  notations  of 
Chapter  II. 

The  eguations  of  motion  are  given  in  Appendix  II  (Equations 
(A2-6)  and  (A2-7 ) )  as  follows: 

T1  =  °1191  +  D1292  +  D(®2  +2W    +   D1 


T2  =  D1291  +  D2292  "  D(91  }  +  D2 


(3.3.1  ) 


where 


2      2 
D11  =  (mi  +  m25  d1  +  m2d2  +  2m2d1d2  cos(92) 

2 
D12  =  m2d2  +  m2d1d2  cos(e2) 

D22  =  m2d2 
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D  =  -  m2d1d2  sin(62) 


D1  =  (m1  +  m2)  gd1  sin^)  +  m2gd2  sin(6  +  0  ) 
D2  =  m2gd2  sin(ei  +  92) 


and  where  g  is  gravity;   T,,  T.,  are  generalized  torques  at  the  first 
and  second  joints  respectively;  and 


m1 ,  d1  and  m   d   are  th 


e  mass 


and  length  of  the  first  and  second  links  respectively. 
Using  the  notations  in  Chapter  II,  let 


JCXj)  = 


D11    D12 


°12    D22 


W(X   X  )  = 


X2  G1(X1)  X2 


I2  q2(v  h 


W, 

L  CJ 


G  (Xt ) 


0    D 


D    D 


G2(X1) 


-D    0 
0     0 


£0^)  = 


Si  =  (er  V 


h 


1  =  (Tr  T2) 
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and  notice  that   D,  D.   and   D.  .   for   i,  i  =  1,  2,   are  functions  of 
X  .   Equation  (3.3.1 )  may  be  rewritten  as  follows: 


t  =  jq^  )  x2  +  f  (x^,  x2: 


f  (Xjj  ,  x2 )  =  i  (^  )  +  w(x  ,  x2 ) . 


(3.3.2) 


The  DC  motors  are  modelled  in  a  method  identical  to  that  in  Section 
2.3,  i  .e  . , 


JX  2  +  AX  1  +  BX  2  +  T  =  CX 

£3  =  F1*2  +  F2*3  +  L"1^ 

where  X_   is  the  vector  of  armature  currents  (2X1 ),   V  is  the  vector 

of  applied  armature  voltages  (2X1 ),  and  where   J,  A,  B,  C,  F  ,  F 

1    2   and 

L    are  as  defined  in  Section  2.3.   Following  Section  2.4,  the 
augmented  model  for  the  two  link  with  actuators  is  finally  given  by 


*1    =*2 

£2-a«)  (3.3.3) 

*3  =  ?&  *  %  +  l"1v 


where 


g(x)  =  j    (^  )[cx3  -  wqc^ ,  x2)  -  z(x^ )  -  ax 1  -  ek  : 
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Using  the  transformation 


*  "5< 


^2  =  *2 


I3  =  g(x) 


we  obtain  the  FLDT  by  applying  Equations  (3.1.16)  and  (3.1 .17)  as 
follows: 


sl/v* 


£(Y. 


(m1    +m2)  gd1    Y21    cos(Yl1)    +  m2gd2    (Y^    +  V„  ),.<** 
m2gd2(Y21    +   Y22)    cos(Yl1     +   Y12) 


LHW 


M 


W(Y)      = 


W. 


2D[Y22    Y32    +   Y31     Y22    +    Y21     V    +D[Y22+2Y21       Y22] 


-2D[Y21     Y31]    -    D[Yj    ] 


D-  -™2d1d2Y22       cos(Y12) 


D  =   ~m2d1d2        sin(Y12} 


J(Y    )    = 


°11  °12 


D12  ° 


°11    =   -2m2d1d2Y22    si»<Y12> 
D12   =   -m2dld2Y22   sin(Y12) 
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and  the  FLDT  voltages  may  be  explicitely  given  as  follows: 


V1  =  lyT-   %  ♦  ^  «,  ♦  D12Z2  +  Wl  ♦  Dl  +  -(Wl  +  H,  )  ♦  a  Aiyii 

2 

1     T      V  R  R  ,   V1 

+  lS  +  — —  +  z;  VY2i  -  [b1  +  iu  +  J-(Dl1  +  j1")]y31 
R1 

+    [°12    +^V    Y32}  (3.3.4) 

and 

f-> 

V2    =  r^h    {D12Z1    +    (D22    +   V    Z2    +   »2    +   \    +  C<W2    +   °2  '    +  IT  A2Y12 


-2      *  «  "2 


'KS  *2         ,  .  •  *2 


+    [A2+— 4~     +   uV    Y23    +    [D'l2+lf  D12]    Y31 

/V.  v^* 

R2 
+    [B2    +I^(D22    +   J2)]    V  (3.3.5) 

where  all  the  parameters  used  are  as  defined  in  Section  2.3,  and  where 
Z1   and   Z2   are  the  angular   jerk  input  of  the  first  and  second  link 
respectively. 

When  V1   and   V2   as  obtained  in  Equations  (3.3.4)  and  (3.3.5) 
are  applied  to  the  system  (3.3.3)  we  obtain  the  EOS  as  two,  decoupled 
triple  integrator  systems  in  the  form  of  Equation  (3.1.19). 

As  in  the  previous  section,  an  experiment  was  conducted  to  evaluate 
the  step  responses  of  the  EOSs  (shown  in  Figures  7  and  8 )  as  follows: 
Experiment  2.   A  FORTRAN  program  (EXP2),  which  simulates  a  double  link 
manipulator  system  driven  by  two  DC  motors  (PMI  MC23S  and  PMI  MC1  9P 
for  the  first  and  second  links  respectively)  (Equation  (3.3.3)),  was 
written.   (Its  listing  are  given  in  Appendix  III.)   The  input  voltages 


51 


were  computed  via  the  FLDTs  (3.3.4)  and  (3.3.5)  for  a  jerk  step  of 

2 
20  rad/S   and  zero  initial  conditions  for  both  links,  with  the 

following  parameters: 


m1    =  m2    =   10    [kg] 
d1    -  d     -  1     [m] 


K        =  K        =   0.5    [V    •    S/RAD],     [Nm/A] 

1  *1 

K        =  K        =   0.25     [V    •    S/RAD],     [Nm/A] 

2  f2 


S,  =    1     [fll 

R2  =   0.6    [0] 

L  =   0.00025    [H] 

L2  =    0.00015     [H] 

ri    =  r2    "   35 


J.,  =  0.002  [kg-m2] 
J2  =  0.001  [kg-m2] 


The  step  responses  of  the  armature's  voltage  and  current  and  the  links' 

angular  velocity,  acceleration  and  position  for  the  first  and  second 

links  are  shown  in  Figures  7  and  8  respectively. 

As  can  be  seen  from  Figures  7  and  8  the  EOS  has  a  step  response  of 

two  decoupled  triple  integrator  system.   Notice  also  that  as  in 

Experiment  1,  all  nonlinear! ties  are  "absorbed"  in  the  armature's 

currents  ( ^  ,  ^ )  and  voltages  (V^  V^).   By  the  fact  that   ^   is 

different  from   I   and   V   is  different  from   v   whereas   9=9, 

2  12 

W1  =  w2,  W1  =  W2,  we  observe  that  all  coupling  effects  are  also 

perceivable  through  the  currents  and  voltages  only. 
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Figure  7.   First  Link,s  step  Response 
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Figure  7.   Continued 


54 


i 
1 

• 

c 

| 

I" 

i 

! 

..     

1 

L 

i 

i 
i 

1 

i 
j 

! 

j 

i 

1 

i 

i 
i 

! 

o 
to 

i 

i 

i 

1 

_ j 

i 

i 

i 

»\l 

----J 

-4 

i 

1  S 

/ 

a 
era 

j 

i 

?:  ~ 

- r--' 

■s 

x\ 

i 

»— o 

1 

j 

1 

i 

i 

— j — 

--{■— 

o 
ru 

! 

o 

| 

V- 

— |— 

■■■'•—  ■ 

s 

tl 

V 

i 

— j— - 

■[--- 

1— 

-\ 

o 

! 

:/ 


'o.oa  o.io  o.?o  n.ia  o.io  a. 5a  g.eo  0.70  o.»o  o.9a  i.oo 


Figure   7.      Continued 


55 


on 

1 

; 

~~T"  "  ~" 

» — 

——a 

^-_   „_  _ 

I  

H 

1 

J 

i 
| 

| 

•:N 

\      . 

a 

»|      '  - 

1    

~j 

- 

1 

1<T 

j 

\ 

\— - 
\ 

^xf 

i 

9 

■ 

'      \ 

...... 

* 

■■■   ; 

^ 

1 

• 

' 

\ 

-"1 

^■•^^ 

1 

1 

•  --« 

^jf*~ 



i 

kl 

— j— ■      " 

—  {.„ 

-' 

— ;  „. 

_..; ..._ 

o 

(Tl 

D 

- ;-- 

•- 

•---■] 

"j 

i — 

...;.... 

-    . . 

.  .i — 

n 

C3 

1— 



!- 

---• 

_„i._„ 

.     ,.  . 

I  -. . 

■    »     -      - 

-  •  i 

U> 



C7 

r-  - 

-  ■ 

'o. 

mi            o. 

0 

0. 

W                 0. 

in            o. 

to             a. 

O 

i 

I 

..... 

.  J „ 

_  | 

i 

i 

| 

g 

.    ...j. 

— I 

j 

i 
/ 

■ 

•  \ 

a 

i    -     . 

! 

..... ; — 



I     ■-; 

/ 

1 

j 

1 

1 

■    \f-" 

r1        ' 

-j— - 

1 — 

O 

J 

: 

j ■■ 

yS{ 

• 

i 

| 

o 

S' 

- | - 

— j - 

—   f  — 

■■  :  - 

a  ■ 

^f. 

^sr^lZ.. 

1 

™~j 

......  |  ...... 

,- 

1 

!  - 

a 

- \ - 

" j 

1 

--4— 

\  — 

.... ; 

o 

; 

a. 

no             0. 

10                  0. 

N 

0. 

ill                0. 

no 

0. 

50                   0. 

99 

sec. 


Figure  8.   Second  Link's  Step  Response 
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CHAPTER  IV 
TIME  OPTIMAL  POSITIONING  CONTROL  (TOPC) 

Having  obtained  the  form  of  the  feedback  linearizing  and  de- 
coupling transformation  in  Chapter  III  for  the  system  model  we  are  now 
ready  for  the  solution  of  the  time  optimal  positioning  control  problem. 

Section  4.1  presents  the  mathematical  statement  of  the  time 
optimal  positioning  control  for  the  N-DOF  manipulator  with  state 
variable  inequality  constraints  and  the  difficulties  encountered  in 
the  direct  solution.   In  Section  4.2  the  FLDT  derived  in  Chapter  III 
is  applied,  then  the  optimal  control  problem  is  restated  and  discussed. 
The  solution  to  each  one  of  the  resulting   N  uncoupled  subsystems  is 
presented  in  Section  4.3  for  various  cases.   Finally  in  Sections  4.4 
and  4.5  a  single  and  double  link  manipulator  examples  are  solved 
respectively. 

4.1   Mathematical  Statement  of  the  TOPC  Problem 
The  time  optimal  positioning  control  problem  for  the  N-DOF  manipu- 
lator may  be  stated  as  follows. 

Given  the  system  defined  in  Equations  (2.3.5),  (2.3.6)  and  (2.3.7) 
with  the  following  initial  conditions: 
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V0)  -.£,0 

*2(0)=°  (4.1.1) 

$<0)  =  C-1U(X1(),  0,  0)  +  AX1Q]  ; 

find  the  minimum  time  tf,   and  an  input  voltage  vector  V(t),  . 
te[0,  tf]   to  be  applied  to  the  system,  such  that 

W=*if 

W"*  (4.1.2) 

x3(tf)  =  c~1[f(x1f,  o,  0)  +  ax  ] 

and  for  all   te[0,  t  ] 


(4.1  .3) 


lX2il 

< 

-0) 

M. 

l 

|*2i 

dt 

Is. 

M. 

l 

(4.1  .4 


and 

d2X2(t) 
l~^~l~Ai  (4.1.5) 

for     i   =   1  ,    2,    .    .    .    n. 

Notice  that  the  manipulator  is  at  rest  (static)  in  both  boundary 
points  (Equations  (4.1.1)  and  (4.1.2))  since   X_2   and   -^   are  both 
zero  at  t  =  0   and  t  =  tf .   Thus,  the  problem  is  to  find  the  optimal 
control  for  time  optimal  point  to  point  positioning,  while  observing 
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inequality  constraints  on  the  velocity,  acceleration  and  jerk  of  the 
manipulator's  motion. 

Let  us  now  demonstrate  the  various  difficulties  encountered  when 
a  direct  solution  via  the  maximum  principle  is  attempted. 

Form  the  Hamiltonian,  H   (following  [A-3] ) 

H  "  1  +  A  '  *2  +  -2  9-(^«  t)  +  A^^  +  F2-3  +  L_1^]  (4"1  -6) 

The  adjoint  variables  ^(t),  ^(t),  ^(t)  e  RN  must  satisfy 

dXi     3H 

dt  "  "  3x       1=1,2,3  (4.1  .7) 

— i 

According  to  the  maximum  principle,  the  optimal  control   V*(t), 
the  optimal  trajectory  X*(t)   and  the  optimal  adjoint  variable's 
vector  X    (t)  =  (A^  ,  X ^  ,    \^)        must  satisfy  the  necessary  conditions 
(2.3.5),  (4.1  .7)  and  the  relation  (4.1  .8) 

H(X*(t),  X*(t),  V*(t),  t)  -  H(X*(t),  X*(t),  V(t),  t)  (4.1.8) 

for  all   V(t)  eR   and  t  e[0,  tf ]   whenever  the  inequality  constraints 
(4.1.3),  (4.1.4)  and  (4.1.5)  are  strictly  satisfied.   On  a  constrained 
arc,  however,  (i .e .,  when  either  one  of  the  inequalities  becomes  an 
equality)  it  is  not  at  all  clear*  how  to  replace  relation  (4.1  .8). 


#T 
In  certain  cases,  e  .g ,  a  scalar  state  inequality  constraints, 
necessary  conditions  on  a  constrained  arc  are  given  in  [S-4]  and 
[M-4].   For  the  general  case,  however,  no  theory  could  be  found 
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At  this  point  the  following  observations  should  be  noted. 

The  number  of  constrained  arcs  and  the  time  instances  at  which 

we  enter  to  and  exit  from  constrained  arcs  are  not  known  and  no 

theory  is  available  for  finding  them  analytically. 

The  set  of  necessary  conditions  to  be  satisfied  by  an  extremal 

trajectory  on  a  constrained  arc  is  not  known  (no  theory). 

Even  on  an  unconstrained  arc  with  given  points  of  entry  and 

exit,  the  resulting  two  point  boundary  value  problem  is  a  high 

order,  (36  variables  for  a  six  DOF  manipulator)  nonlinear,  highly 

coupled  problem. 

There  may  well  be  many  time  optimal  trajectories,  i.e.  uniqueness 
is  not  at  all  guaranteed. 

In  view  of  the  above,  it  is  concluded  that  the  direct  attempt  to 
solve  this  problem  is  a  formidable  task,  thus,  in  the  next  section  we 
introduce  a  new  statement  of  the  TOPC  problem,  utilizing  the  FLDT 
derived  in  Chapter  III,  which  enables  the  unique  solution  of  a 
restricted  TOPC  problem. 

jk2  Statement  of  a  Restricted  TOPC  Problem  with  FLDT 
In  this  section  we  employ  the  results  of  Chapter  III  by  assuming 
that  the  N-DOF  manipulator  system  was  linearized  and  decoupled  into  N 
triple  integrator  subsystems  each  representing  a  single  DOF  of  the 
manipulator  by  application  of  the  FLDT.   Then  the  TOPC  problem  is  re- 
stated and  shown  to  be  a  restriction  on  the  original  TOPC  problem 
stated  in  Section  4.1. 
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As  shown  in  Chapter  III,  when  the  FLDT  given  in  Equation  (3.1.16) 
is  applied  to  the  system  described  in  Equations  (2.3-5),  (2.3-6)  and 
(2.3.7),  the  set  of  N  uncoupled,  linear,  triple  integrator  subsystems 
given  in  Equation  (3-1-19)  becomes  the  equivalent  open  loop  system. 

Let  us  now  state  the  following  optimization  problem,  which  will 
be  denoted  as  the  restricted  TOPC  problem.   For  each  one  of  the  N 
subsystems  described  by  Equation  (3.1.19),  with  the  initial  conditions 


Yu(0)  ■  V 

Y2.(0)  =  0  (4.2.1) 

Y31(0)  =  0  , 

* 
find  the  minimum  time  t   ,  and  the  optimal  jerk  input  Z  (t)  to 

i 

be  applied  to  the  system  on  t£[0,  t„  ]  such  that 

i 

Yu(tf    >    =X1f 
l  l 

Y      (t      )    =   0  (4.2.2) 

i 

Y3i(V   =  ° 


and  for  all     t  e  [0,    t     ] 


Ki\    -  "M  (4.2.3) 


i 


l*3i|-V  «-2-«) 


i 
and 


lZi I    -  Ai  (4.2.5) 
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for  i  =  1,  2,  .  .  .  N,  where  X     and  X     are  the  i-th  compo- 

i         i 
nents  of  X    and  X    in  Section  4.1  respectively. 

The  above  stated  uncoupled  set  of  optimization  problems  with 
state  variable  inequality  constraints  (SVICs)  is  closely  related  to 
our  original  TOPC  problem  stated  in  Section  4.1  as  follows.   Recall 
from  Chapter  III  that  Y  =  X   is  the  position  vector  of  the  manipu- 
lator  joints,  and  that  Y_2   =  X_2  and  Y  =  g(X)  are  the  manipulator 
velocity  and  acceleration  vectors  of  the  manipulator   joints  where 
g  is  given  in  Equation  (2.3.6).   Therefore,  it  is  clear  that  the 
initial  condition  (4.1.1)  is  identical  to  (4.2.1),  that  the  terminal 
condition  (4.1.2)  is  identical  to  (4.2.2)  and  that  the  inequality  con- 
straints in  Equations  (4.1.3),  (4.1.4)  and  (4.1.5)  are  identical  to 
those  given  in  (4.2.3),  (4.2.4)  and  (4.2.5)  respectively. 

In  view  of  this  similarity  it  is  clear  that  if  a  solution  to  the 
restricted  TOPC  problem  is  given  in  the  form 

Z  (t)  =  (Z   Z        .    .    .   1   )l  (4.2.6) 


'V   "2»  -  *  '  -H' 


where 


_* 


i 


Z.     0  -  t  -  t 
i  f . 


Zi  =  \  (4.2.7) 


t   <  t 

i 


then  substitution  of  Z   in  the  FLDT  (3.1.16)  yields  the  optimal  input 
voltage  V  (t)   sought  for  in  the  TOPC  problem,  where  t   is  given  by 


#0 
See  for  example  the  discussion  of  a  similar  case  in  [L-1] 
pages  53  -  56. 


64 


tf  =  maxttf  ]  ;   i  =  1,  2,  .  .  .  N  (4.2.8) 

i    i 

In  other  words,  we  find  that  an  optimal  solution  to  the  restricted 
TOPC  problem  is  automatically  (via  the  FLDT)  an  optimal  solution  to  the 
TOPC  problem.   The  opposite  relation,  however,  does  not  necessarily 
hold,  since  the  optimal  solution  to  the  TOPC  problem  does  not  necessi- 
tate that  each  and  every  one  of  the  degrees  of  freedom  of  motion  will 
move  in  minimum  time . 

It  is  intuitively  clear  that  the  restrictions  of  this  type,  i.e., 
multiobjective  optimization,  enable  us  to  obtain  uniqueness  which  we 
may  not  otherwise  have  had.   In  our  case,  due  to  the  desirability  of 
an  uncoupled  control  strategy  for  thfe  various  DOFs,  this  restriction 
is  recommended  and  from  now  on  by  TOPC  problem  we  shall  mean  the 
restricted  TOPC  problem  stated  in  this  section. 

4.3   TOPC  with  SVIC's  for  Triple  Integrator  System 
As  a  result  of  the  restatement  of  the  TOPC  problem  via  the  FLDT, 
we  now  deal  with   N  independent  TOPC  problems.   In  this  section  we 
find  the  time  optimal  positioning  control  for  various  tasks  depending 
upon  which  of  the  SVICs  is  active. 

We  commence  by  defining  a  generic  triple  integrator  system  for 
which  we  find  the  time  optimal  positioning  control  to  the  origin 
without  state  variable  ineguality  constraints.   Then  depending  upon  the 
task  at  hand,  and  by  exploiting  a  very  useful  property  of  the  electro- 
servomechanism  system,  we  define  a  variety  of  special  cases  with  regard 
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to  the  "activity"  of  a  state  variable  inequality  constraint.   For  each 

one  of  these  cases  the  TOPC  problem  is  then  solved  separately. 

For  each  i-th  subsystem,  let 

K    =  X1f   "  X10                                               (4.3.1) 
1      i 

then  the  restricted  TOPC  problem,  without  SVICs  for  each  subsystem, 

may  be  stated  as  follows.   (Without  loss  of  generality  we  shall  assume 

that  £  >  0   for  all  subsystems  throughout  this  work.) 

Problem  P1  .   Given  the  system  I 

X'l  =  X2 

• 

E:    X2  =  X3                                                 (4.3.2) 

X.  =  Z 
3 

. 

and  the  initial  conditions 

x1  (0)  =  -C 

(4.3.3) 

x2(0)  =  x3«>)  =  o 

find  the  minimum  time   tf   and  the  optimal  jerk  input   Z*(t),  t  e  [0,t  ] 

such  that 

VV  ■  VV  ■  VV  =  °                                  (4.3.4) 

and 

|z(t)|  5  A    vt  elO,  tf]                             (4.3-5) 
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The  problem  P1  is  a  special  case  of  the  well  known  time  optimal 
control  to  the  origin  of  a  triple  integrator  system,  for  which  a  solu- 
tion is  given  in  terms  of  a  switching  surface,  F  (see  for  example 
[  F-1  ]  and  [  S-3  ]  ) 


F  =  X1  +  X^/3  +  e  X2X3  +  e[X^/2  +  e  X2]3/2 


(4.3.6) 


where 


e=  sgn(X2  +  X  /2), 


and  where  the  optimal  control  is 


Z  =  -A  sgn(F) 


(4.3.7) 


In  the  case  of  P1 ,  however,  due  to  the  zero  boundary  velocity  and 
acceleration  conditions,  it  was  possible  to  obtain  a  closed  form  repre- 
sentation of  the  optimal  control,  and  the  state  trajectories  as  follows. 
Theorem  4.1.   The  optimal  control  for  P1  is  given  as  follows: 


r 


o  5t<  -L 

4 


Z  (t)  =  /  -A      -i 


f  <     3tf 
4-  "  t   <   — 


(4.3.8) 


^  <  «■  < 
—  -  t   ~   fcf 


where 


««  ■  *«¥' 


1/3 


(4.3.9) 
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Proof-      The   system,       Z,      is   fully  controllable   since 


Rank 


b 

0 

1 

j 

0 

1 

o~ 

ro~ 

0 

0 

1 

0 

0 

0 

0_ 
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1 

0 

1 

0" 

0 

0 

1 

0 

0 

0_ 

b 

1 

o" 

o' 
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0 

1 

0 

p 

0 

0_ 

J. 

=   3 


in  addition  there  is  a  single  input,   Z,   thus  the  system   Z  is 
normal  (in  the  sense  of  definition  4.15  in  [A-3]  page  218)  and  therefore 
the  solution  to  P1  exists  and  is  unique  [A-3].   It  is  therefore  suffi- 
cient to  find  one  control  function,  which  together  with  its  resulting 

state  and  adjoint  variables  trajectories  satisfy  the  necessary  condi- 

# 
tions  for  optimality. 

* 
When   Z  (t),  as  defined  in  (4.3.8)  and  (4.3.9),  is  applied  to  the 

system   I  then  the  acceleration  X3,  the  velocity  X2   and  the  position 

X1   trajectories  are  found  by  simple  integration  as  follows. 


x3(t) 


At 


A(--t) 


A(t    -   tf  ) 


0    it    </ 

4  4 


3t 


f    <  i.    < 

-  -   t    -   t. 


(4.3.10) 


X    (t)    = 


r 


At' 


At 


_f 

16 


"  2(t  "  T- 


f    ,2 


A(t   -   tf)' 


o-<t<3I 

4  4 


3tf    <        < 
—  "   t   ~   fcf 


(4.3.11  ) 


Satisfying  the  necessary  conditions  will  only  indicate  extremalitv 
In  our  case  however.it  will  be  shown  that  it  is  a  minimum. 
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r 


x1  (t) 


At 

-5  +    6 


°  -<  t  <  / 


Atf         At, 


-2  .  n  t  +  £l  t2     dL.        ^  <       3tf 


16    "    '       4 
2 


26 


v     4 
3  3t 


192 


r,  Afcf  Afcf 

t      +  —  t    -   — —  ^2         At 


f<t^t. 


V. 


(4.3.12) 


Following    [A-3],    let   the   Hamiltonian   function   be  defined  by 


H   =   1     +   XlX2    +   X2X3    +   X3Z 


(4.3.13) 


then  the  adjoint  variables   X. (t) 


i  =  1,  2,    3  must  satisfy 


1  ax 


K2   ~        6X2   -   -   X1 


(4.3.14) 


X      -        25--         X 

x3  ~       ax3  -  "  X2 


From    (4.3.14)   we   find   that 


x1  (t) 


10 


x2(t) 


-   X10fc   +X20 


(4.3.15) 


X3(U    =Mf--   hdt   +X30 


where      Xi()   =  X^O)      ;        i   =   1,    2,    3      and      te[0,    t    ] 


Applying  the  maximum  principle    [A-3]    we   find   that      Z*(t)  must  satisfy 


Z    (t)    =   -   A  sgn(X    (t)) 


(4.3.16) 
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but  from  (4.3.8)   z*(t)   has  two  switching  times,  at   t  =  —  and 

3t  4 

at  t  =  — — ,  thus 


X3(t)  -K  (t-/)  <t--/)  (4.3.17) 

Equating  coefficients  in  (4.3.17)  and  (4.3.15)  we  find  that 


X10 

— 

2K 

X20 

= 

K    • 

fcf 

X30 

= 

3 
16 

-J 

in  particular,  for   t  =  0+,   we  obtain 


H(0  +  )  =1  +  X1  0  +  X2  0  +  \        A  =  0 


or 


K  = 


3  A  t: 


(4.3.18) 


It  is  also  known  ([A-3])  that   H(t)  must  satisfy 

H(t)=0     VtE[0,  tf]  (4.3.19) 


X   -   1 
30  "   A  (4.3.20) 

In  view  of  (4.3.18)  we  obtain 

2 
3K  A  t 

1  +— IT"  =° 

or 

16 


(4.3.21  ) 
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To  summarize,  the  canonical  (adjoint  plus  state)  variables  which 
correspond  to  the  control   Z  (t)   in  (4.3.8)  are  given  by  (4.3.10) 
through  (4.3.12).    Since  they  satisfy  the  Euler  Lagrange  eguations 

(4.3.2)  and  (4.3.14),  the  condition  (4.3.19)  and  the  maximum  principle 

* 
(4.3.16),  then   Z  (t)  must  be  optimal  control.   Notice,  that  the 

minimum  time  t   was  found  by  letting  X  (t  )  =  0   in  Eguation 

(4.3.12)  which  yields 

« 

4C  V3 
tf  =  *££>  Q.E.D. 

The  optimal  control,  state  and  adjoint  variable's  trajectories 
for  problem  P1  are  respectively  described  in  Figure  9. 

Thus  far  we  only  obtained  the  TOPC  for  the  unconstrained  state  case. 
In  the  presence  of  SVICs  the  previous  method  (maximum  principle )  does 
not  hold  and  the  approach  taken  by  [S-4],  [M-2]  and  [L-2]  will  be  used. 
In  these  references,  however,  only  scalar  SVICs  exist  whereas,  as 
shown  in  Eguations  (4.2.3)  through  (4.2.4),  our  single  DOF  subsystem 
must  satisfy  multiple  SVICs  simultaneously.   This  difficulty  is  removed 
by  the  following  important  observation  concerning  the  appearance  of 
velocity  and  acceleration  constraints  in  positioning  tasks  of  servo- 
mechanisms.   (In  the  seguel  this  observation  is  treated  as  an 
assumption . ) 


No  theory  was  found  to  exist  for  the  vectorial  SVIC  case  with 
scalar  input  systems. 
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Figure  9.   Optimal  Control,  State  and  Adjoint  Trajectories  for  Problem  P1 
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Assumption .   When  accelerating  (decelerating)  from  rest,  an  electro- 
servomechanism  system  will  always  reach  its  maximum  (minimum)  accelera- 
tion before  achieving  its  maximum  (minimum)  speed. 

This  assumption  is  based  on  the  physical  fact  that  the  electrical 
time  constant  of  the  actuator  is  usually  about  two  order  of  magnitudes 
smaller  than  the  mechanical  time  constant  of  the  loaded  actuator, 
therefore  the  actuator  achieves  its  maximum  current  (related  to  torque 
or  acceleration)  much  before  its  maximum  speed  (which  depend  on  the 
load-s  and  actuator's  inertia)  when  the  maximum  voltage  (related  jerk) 
is  applied  to  it. 

Based  upon  the  above  given  assumption  we  now  may  obtain,  depending 
upon  the  task  at  hand  (i.e.,  depending  upon   FJ  one  of  the  following 


cases . 


CaseJ_.   No  SVICs  are  active  and  therefore  the  restricted  TOPC  problem 
is  identical  to  PI  for  which  a  solution  is  obtained  in  Theorem  4.1  . 


(This  case  occurs  if   £  <  £2J!  ,  see  explanation  below.) 

A 

Case_2_.   Constrained  acceleration  only.   This  case  occurs  when  the 
following  conditions  are  both  satisfied. 

o  3 

2aM 

£  >  — - 

A2  (4.3.22) 

and 


4 
a 
M 

ww  >    +  a   •  F 

4A2     M       ~  2A  (4.3.23) 


2 
a 

M 
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Condition  (4.3.22)  simply  means  that  the  maximum  acceleration  allowed 

is  smaller  than  the  highest  acceleration  that  would  be  obtained  for 

the  task   £,  if  the  unconstrained  case  (P1  )  is  assumed.   (See  Figure  9.) 

Condition  (4.3.23)  will  be  explained  below. 

* 
The  optimal  jerk  input   Z  (t )   for  Case  2  i.e.,  when  the  SVIC 


|x3(t)|  -  aM    Vte[0,  tf] 


(4.3.24) 


is  added  to  the  statement  of  problem  P1  ,  is  found  by  applying  the 
method  described  in  [S-4]  to  this  system.   The  resulting  optimal  tra- 
jectories and  the  optimal  control  are  given  in  Eguations  (4.3.25) 
through  (4.3.29)  and  are  also  described  in  Figure  10. 


0    -   t    <  T2 

A 


A  2      ~    A 


Z*(t) 


-A 


fcf         °M    <   ^  fcf         % 

—  -  r  - t  <  —  +  ~K 


tf       "*  <•.*«.'       "m 

-  +  ~J  ~   t    <  fcf    "   "A 


M   <        <      ' 

fcf    -   "A   -   t    -   fcf 


(4.3.25) 
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x3(t) 
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0    <  t    ^ 


A  2      "      A 


fcf         aM   <  ^  fcf         aM 

r  -t  - fc  <  —  +  ~k 


2A  f  A 


M   <        < 

fcf    ~  "A   "   fc    -   fcf 


(4.3.26 


x2(t) 
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2 

a 


M 
2A  M 


t2         fcf 
A[-\  +  -|t 


0<t<* 
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a  t 

JM  <        <     f 

A        Z   ~      2 
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"M.2  Vf. 

"    ("A)       +   Ia"1 


fcf         "M  ■  aM 


L 
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(4.3.27) 


x1  (t) 


where 
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(4.3.28) 


M 


(4.3.29) 


As   shown   in    Figure   10   the  maximum  velocity   is   obtained 
and   is  given   by    (from   Eguation    (4.3.27)) 


at     t   = 
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Figure  10.   Optimal  Control  and  State  Trajectories  for  TOPC 
Problem  with  Bounded  Acceleration  (Case  2) 
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In  order  not  to  exceed  the  maximum  speed  it  is  necessary  for  the 

task   5  and  the  constraints   a  .  w  ,  A  to  satisfy 

MM  ' 


M    2 


max 


which  is  exactly  the  condition  (4.3.23)  which  is  necessary  for  having 
Case  2. 

Case_jS_.   All  SVICs  are  active.   This  case  occurs  when  the  task   £   is 
such  that  condition  (4.3.23)  is  not  satisfied,  i.e.  when 

„  /ttM  ttM 


Again,  as  in  Case  2  the  method  for  finding  the  optimal  control  on  con- 
strained arcs  in  [S-4]  was  used  for  the  restricted  TOPC  problem  (see 
Section  4.2).   The  resulting  optimal  control  and  state  trajectories 
are  given  in  Equations  (4.3.30)  through  (4.3.34)  and  are  also  described 
in  Figure  1 1  . 
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(4.3.30) 


78 

A 

ti 

3          \         S 

i 

2 

fc2          fc3 

-A 

J1 

f6 

it 

aM 

y 

\ 

X3 

/ 

"V, 

N 

/ 

■ 

"M 

"to 

x„ 

2 

X 

1 

f 

\ 

§ 

2 

-c 

Figure    1 1  . 

TOPC  for   Case   3 
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where 


1  A 

t.    --2 


t„    = 


fc1    +t2 


+.             c,  M          M 

*»*      =  —  +  - —  +  — 

f          u  A         a 

M  m 


(4.3.34) 


fc4    =   fcf      ~   fc3 


fc6    =   fcf    "   S 


here    (Ea^atLnTHl  n    *    -f  ^   ^  ^   "-  °Ptlmal   Rectory  obtained 
nere    Ration    (4.3.31))    ia   identical    to   the   trapezoidal   acceleration  motion 

virions  (high  i-'i^i.'S.S'^S  £  SSSS^SL!* 
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Figure  12  describes  the  various  speed  trajectories  obtained  for 
the  above  described  cases  with  the  path   £  as  a  parameter. 

Having  obtained  the  solution  to  the  restricted  TOPC  problem  we 
complete  this  chapter  by  describing  two  examples,  single  and  double 
link  manipulators  in  Sections  4.4  and  4.5  respectively. 

4.4   TOPC  for  a  Single  Link  Manipulator  (Example  1) 
In  this  section  we  apply  the  results  of  Section  4.3  to  a  single 
link  manipulator  system.   A  load  mass  m   is  attached  through  a  rigid 
link  with  length  d  to  the  shaft  of  a  DC  motor,  as  described  in 
Figure  4.   The  task  is  to  rotate  the  load  through  an  angle  of   £ 
radians  in  minimum  time  while  observing  constraints  on  the  maximum 
speed,  acceleration  and  jerk  of  the  motion  as  follows. 

Experiment  3.   The  PMI  MC1 9P  DC  motor  was  chosen  with  the  following 
parameters . 

R  =  0.6  fi 

L  =  0.0015  H 

KT  =  0.25  Nn/amp 

Kv  =  0.25  V/r ad/sec 

JM  =  0.001      Kg  -  m2 

Maximum  Current,   I    =100  Amn 

max         H " 

Rated  Voltage,    v  =  85  V 
R 

Rated  Speed,    w  =  340  rad 
R        S 
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The  load  was 

m  =  10  Kg 
d  =  1  m 

The  gear  reduction  ratio  was 

r  =  100 
and  the  gravity  acceleration  was  assumed  to  be 


g  =  10  — 
S 


The  path  to  be  traversed  was 
£  =  1  rad. 


(All  notations  are  consistent  with  those  of  Chapter  II.) 

The  equations  of  motion  for  the  system  (3.2.2)  were  derived  in 
Appendix  I  and  used  in  Experiment  1  of  Chapter  III.   Those  equations 
with  the  FLDT  given  in  Equation  (3.2.3)  were  simulated  in  a  FORTRAN 
program  called  FIN1  (its  listings  are  given  in  Appendix  III)  with  the 
optimal  jerk  control  as  given  in  Equations  (4.3.30)  and  (4.3.34)  for 
the  TOPC  problem,  Case  3.   m  addition,  the  maximum  speed,  acceleration 
and  jerk  were  found  from  the  motor  specifications  as  follows: 

V 
u     =  _R 85 

M   Kv  r  "  0.25   TOO"  _  3-4  rad/sec  , 
from  the  equations  of  motion  (3.2.2)  we  obtain 
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angular        dX„ 

2  2    2 

acceleration  =  — -  =  [rK  I  -  mgd  sin(X„  )]/(md   +  r  J  ,) 
Qt       1  M 


thus 

dX 


max[-^]    -  «M  -    [rKT  Imax  -  max(mgd   sin(X1  ))]/(md2    +  r2JM) 


or 


25-100-100  2 

aM  =  Jo" =  120  rad/sec   , 

also  the  maximum  jerk  was  estimated  via  the  electrical  time  constant 
(assuming  that  it  takes  about  four  electrical  time  constants  to  develop 
the  maximum  acceleration)  as  follows. 

A  =  Jl—  •=   12°     =  120,000  rad/sec2  (\   *      ^S 

4  R   4   °-6  A-  i^ 

4   L     0.00015 

. 

With  the  above  given  parameters,  the  program  FIN1  simulated  the  system 
motion  from  X1  =  -1  rad   to  the  zero  position.   Figure  13  describes 
the  FLDT  voltage,  the  motor's  current,  the  velocity  and  position 
trajectories  respectively.   As  can  be  seen  in  Figure  13  the  velocity 
and  position  trajectories  identcally  match  the  expected  curves  of  a 
TOPC  for  a  triple  integrator  system,  indicating  a  perfect  linearization, 
the  voltage  and  current  of  the  actuator, however,  demonstrate  the  non- 
linear effects  of  the  load  as  it  changes  configuration. 
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Figure  13.   TOPC  Trajectories  in  Experiment  3 
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4.5   TOPC  for  a  Double  Link  Manipulator  (Example  2) 
An  experiment  similar  to  the  one  described  in  Section  4.4  but  for 
a  two-link  manipulator  is  described  in  this  section.   The  system  is 
described  in  Figure  6 .   The  equations  of  motion  for  the  system  are 
given  in  Appendix  II  and  are  also  used  in  Experiment  2  of  Chapter  III. 
The  task  is:   positioning  a  loaded  double  link  system,  driven  by  two 
DC  motors,  at  its  null  position,  starting  from   -1  radian  in  both  joints, 
in  minimum  time,  while  observing  similar  state  variable  ineguality  con- 
straints as  in  Experiment  3 . 

Experiment  4 .   For  the  second  joint  we  selected  the  same  DC  motor  as 
in  Experiment  3  (PMI  MC19P).   For  the  first  joint  the  motor  was  PMI 
MC23S,  with  the  following  parameters. 


r   -  1  a 

L  =  0.00025  H 

K   =0.5  Nn/A 
1 

ie     =0.5   V/rad/sec 
1 

J        =   0.002    Kg   -  m2 

1 

Maximum  current,    I  =   100    A 

max 

Rated   Voltage,      V        =   1 70   V 
R1 


Rated   speed        oj 
R1 

=   340   rad/sec 

The   loads  were 

m      =  m      =   1 0    Kg 

^           \T 

al    =    a2    =   1    m 
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Equations  (3.3.3),  (3.3.4)  and  (3.3.5)  were  simulated  by  a  FORTRAN 
program  called  FIN2  (its  listings  are  given  in  Appendix  III).   The 
optimal  jerk  inputs   Z.]   and   Z2   were  programmed  as  in  Experiment  3. 
In  addition  the  maximum  speeds,  accelerations  and  jerks  for  both  links 
were  found  from  the  actuator's  specifications  and  the  equations  of 
motion  of  the  system  (3.3.3)  as  follows. 

1      170 

M1  -  Kv  ^   "  "50T  "  3-4  "d/sec 

V 
R    85 

\    =^TI=^=  3"4  rad/SSC 

The  maximum  accelerations  a  and   a    were  estiamted  by  the  current/ 

1         2 

acceleration  equations  (X   in  Equation  (3.3.3)) 


C1    '  r^T"    {[JMl    ^   +   D22    +  Q3    +  2Q  cos<e2)]    6! 


1 

s 

+    [D22    +  Q  cos(92)]    62    +  Q2   sin(91    +  9    ) 

+  Q^    sin(91  )    -   Q  sin(92)    (9*   +  29^)}  (4.5.1  ) 

J2    "  r^T    {[D22    +   2  <*»<V]    91    +    [D22    +   JM  4]    »2 

+  Q2   sin(91    +  92)    +  Q  sin(92)    92}  (4.5.2) 


where 
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D22  "  m2d2 


Q  -  m2dld2 


Q1  -  (mi  +  m2)g  d1  (4.5.3) 


Q2  =  m2d2g 


Q3  -Q,  d/g 


and  where   9^  92   are  the  first  and  second  joints'  coordinates 
respectively.   It  was  found  that  in  order  for  the  currents,   I   and 
I2,   not  to  exceed  their  maximum  values,   100  A,  the  angular  accelera- 


tions a  and   a    had  to  be 

M1         M2 

aM  =  aM  =  55  rad/sec2 
1      2 

for  any  9^  92  e(0,  2H-.   Finally,  the  maximum  jerks,  were  found  in 
the  same  way  as  in  Experiment  3 . 

A  _!^1_   .  4-     =  55,000  rad/sec3         4,= 

1  "   R1   ~"  4  

4  —      0.00025 

L1 


and 


2         55  3       A.   _  <k^  RL 


A2  "  ~r-  =  " O —   =  55>000  rad/sec 

0.00015 


4  f-  4  -  ^;°,_ 


With  the  above  given  parameters,  the  program  FIN2  simulated  the 
system  motion  from   8,  =  92  =  -1  rad   at   t  =  0   to  the  null  position 
61 (tf )  =  92(tf )  =  0.   Figures  14  and  15  describe  the  voltages,  currents 
and  state  trajectories  that  correspond  to  this  motion  for  the  first 
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and  second  link  respectively.   As  can  be  seen  in  Figures  14  and  15 
the  velocity  and  position  trajectories  of  both  links,  identically  match 
the  expected  curves  of  a  TOPC  for  a  triple  integrator  system,  indi- 
cating complete  decoupling  and  linearization,  the  actual  voltages  and 
currents  of  the  two  motors  however,  demonstrate  the  expected  non- 
linear and  coupling  effects  through  the  motion. 

In  Figures  14  and  15  the  actuators'  currents  trajectories  may  give 
the  wrong  impression  that  we  actually  produce  a  "bang-bang"  current 
(torgue)  control.   This,  however,  is  not  true.   The  misleading  form  of 
the  current  is  the  result  of  the  fact  that  both  joints  traverse  the 
same  path  with  the  same  velocity  and  acceleration  constraints.   In 
order  to  demonstrate  this,  the  following  experiment  was  performed. 

Experiment  5.   The  program   FIN2  was  used  to  repeat  Experiment  4 
with  0^0)  =  -1   radian   and   e^O)  =  -5  radian  while  all  the  other 
parameters  were  the  same  as  in  Experiment  4.   The  resulting  voltages, 
currents,  velocities  and  positions  of  both  joints  are  described  in 
Figures  16  and  17  respectively.   As  shown  in  Figures  16  and  17,  the 
actuators  currents  no  longer  have  the  "bang-bang"  shape  of  Experiment  4. 

4.6   Comparison  with  Approximated  Bang-Bang  Voltage  Control 
In  order  to  demonstrate  the  advantages  of  FLDT  in  the  slewing  mode 
of  TOPC,  and  to  emphasize  the  effects  of  nonlinearities  and  coupling 
terms  in  the  dynamics  of  manipulator,  (if  they  are  not  accounted  for,) 
the  results  of  Experiment  4  were  compared  to  those  of  the  current 
approach  in  bang-bang  voltage  control  for  slewing  mode  motion,  as  des- 
cribed in  Experiment  6 . 
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Figure  14.   TOPC  Trajectories  of  1st  Joint  in  Experiment  4 
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Figure    14.      Continued 
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Figure    15.      Continued 
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Figure  16.   TOPC  Trajectories  of  1st  Joint  in  Experiment  5 
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Figure  16.   Continued 
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Figure  17.   TOPC  Trajectories  of  2nd  Joint  in  Experiment  5 
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Figure    17.      Continued 
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Experiment  6 .   In  the  present  bang-bang  voltage  control  of  servo- 
mechanisms  in  the  slewing  mode,  the  maximum  voltage  available  is  applied 
to  the  armature  whose  current  is  limited  by  the  hardware  used.   The 
path  traversed  during  acceleration  from  rest  to  the  rated  speed  is 
recorded, and  later  used  as  an  estimate  for  the  stopping  distance. 
When  the  joint  position  is  egual  to  the  stopping  distance,  the  applied 
voltage  is  switched  to  its  highest  negative  value,  for  deceleration. 
When  the  joint's  velocity  or  the  position  error  becomes  zero  a  mechanical 
(or  electrical)  "clutch"  is  used  to  lock  the  joint. 

The  procedure  above  was  simulated  in  a  FORTRAN  program  BAD  (its 
listings  are  given  in  Appendix  III)  as  a  control  algorithm  for  a  double 
link  manipulator.   All  parameters  used  were  identical  with  those  of 
Experiment  4.   (For  comparison,  Figures  18  and  19  describe  the  voltages 
currents  velocities  and  positions  of  the  first  and  second  joints 
respectively. ) 

The  large  positioning  errors,  and  the  coupling  effects  are  clear. 
Figures  18  and  19  show  that  the  terminal  values   6  (t  )  =  -0.12  rad 
and  e^(tf)   =  -0.33  rad  which  are  unacceptably  large.   Also,  the  non- 
smooth  variations  in  the  second  joint's  velocity  after  the  first  joint 
stopped  indicate  the  undesirable  effects  of  coupling. 

Of  course  in  the  general  multi-DOF  linkage,  the  expected  results 
from  such  a  simplistic  approach  will  be  much  worse,  and  the  system 
response  becomes  almost  unpredictible . 


100 


01,11     °-°"     oo«     o.ii     o.is     o.?o     o.a     o.n  o  jj     o»     oga 


™ 

"TZV.. 

J 

j 

! 

i 

i 
i 

8 

i 

j 

I 

1 

i 

j-  - 

B 

\i 

! 

1 

"  T~ 

i 

1  • 

=»*4— - 

| 

!   • 

! 

1 

|— - 

! 

—     t 
i 

i 

1 

o 

1 

g 

i 

! 

l 

j 

j 

i 

8 

o 

iH*wi 

W      * 

f 

1 

.... 

I 

B 
O 

C»  - 



j - 

Q 

j 

8 

f 

— 1 — 

t 
j 

i 

— -|— - 

i 

f  1 

' r- 

— i [■ 

i 

\ 

i 

! 

1 

i 

1 

f                 0.( 

m             o. 

2                 0. 

6                 0. 

0 

0. 

N 

■■ "  "t- 
0.. 

1 1- 

«                 0. 

!                 0. 

sec. 


Figure  18.   Voltage  Bang  Bang  Control  Trajectories  of  First 
Joint  in  Experiment  6 
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Figure  19.   Voltage  Bang  Bang  Control  Trajectories  of 
Second  Joint  in  Experiment  6 
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Figure    19.      Continued 
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CHAPTER  V 
REALIZATION  OF  THE  FLDT 

In  this  chapter  we  discuss  several  aspects  of  the  difficulties 
encountered  when  the  feedback  linearizing  and  decoupling  transformation 
which  was  derived  in  Chapter  III  is  implemented. 

As  can  be  seen  from  Equation  (3.1  .16),  for  a  large  number  of  DOF, 
the  FLDT  may  involve  the  evaluation  of  thousands  of  additions,  multi- 
plications and  trigonometric  operations.   On  line  computations  of  the 
FLDT  may  therefore  take  many  minutes  for  a  single  iteration  and  turn  out 
to  be  impractical  for  real  time  control.   This,  however,  is  not  the  only 
possibility  for  the  FLDT  execution .   Various  combinations  of  evaluations 
of  analytical  expressions  and  references  to  table  look-up  are  possible 
as  a  method  of  computational  burden  reduction  in  each  cycle. 

For  a  given  manipulator  system  the  optimal  selection  of  the 
number  of  parametric  variables,  P  (see  Chapter  I),  for  computational 
efficiency  and  memory  size  reduction  for  real  time  control,  should  be 
assumed  as  an  essential  step  in  the  controller  design  process . 

In  any  realistic  FLDT  implementation,  however,  the  accuracy  in 
the  control  signals  evaluation  is  limited,  and  the  actually  applied 
input  deviates  from  its  desired  value.   It  is  our  goal,  in  this  chapter, 


104 


105 

to  describe  two  results  concerning  the  sensitivity  of  the  manipulator 
output  motion  to  imperfect  (quantized)  control  signals  in  Section  5.1 
and  the  asymptotic  stability  of  the  manipulator  system  in  the  presence 
of  analytic,  partially  known  deviations  of  the  applied  FLDT  from  its 
nominal  value,  in  Section  5.2. 

5.1   Sensitivity  of  Output  Motion  to  Quantized  Voltage 
As  previously  mentioned,  in  any  realistic  implementation  of  the  FLDT, 
some  deviation  of  the  control  signal  from  its  desired  value  must  occur. 
In  the  case  where  some  of  the  parameters  of  the  FLDT  are  stored  in  a 
finite  word  length  memory  addresses  the  total  input  deviation  may  be 
represented  as  a  variable  quantization  error.   However,  due  to  the  tre- 
mendous mathematical  difficulties  involved  in  analysing  this  quantization 
error  variation   as  a  function  of  its  arguments,  simplifying  assumptions 
such  as  assuming  an  approximately  constant  quantization  level  on  the 
control  signal,  had  to  be  made. 

In  what  follows  we  describe  two  sets  of  experiments,  by  which  we 
find  the  sensitivity  of  the  output  motion  of  a  single  and  double  link 
manipulator's  TOPC  to  various  quantization  levels  of  the  FLDT  voltages. 
The  quantization  error  was  simulated  in  the  followinq  form.   After 
the  FLDT  signal,  V,  had  been  evaluated  via  Equation  (3.1.16)  it  was 
shifted  and  truncated,  using  FORTRAN  built-in  functions,  as  follows 

W  =  AINT[0.5  sgn(V)  +  1}    R  (5>1  j 
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where 

R  =   Desired  quantization   level   in   volts, 

AINT(X)    =   sgn(X)    [X], 

[X]  =  Largest  integer  value  of  -  |x | , 

1      X  0 

sgn (X  )  =    0     X  =  0 

-1     X  0 


and 


W  =  quantized  voltage. 


Notice  that  by  shifting  -  by  ijsgn(V)   it  was  possible  to  eliminate 
the  truncation  bias  at   V  =  0.   This  form  (5.1 .1 )  of  quantization  model 
was  used  in  the  experiments  as  follows. 

Experiment  7.   The  program  FIN1  was  used  to  repeat  the  TOPC  for  a 
single  link  manipulator  as  in  Experiment  3,  seven  times,  with  the  following 
values  for   R. 

R  =  1 ,  2,  3,  5,  10,  20,  50  volts. 

In  each  run  the  joint  position  and  velocity  were  recorded  at  t  =  t  . 
Since  the  terminal  point  was  supposed  to  be  the  state  space  origin, 
those  values  represented  the  error  due  to  quantization.   Figures  20 
through  26  describe  the  quantized  voltages,  the  actuators'-  currents, 
the  output  velocities  and  joint  positions  for  all  values  of   R.   in 
addition,  the  resulting  terminal  position  errors  were  described  as 
a  function  of   R  in  Figure  27. 
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Figure   20.      Continued 
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Figure  21.   TOPC  Trajectories  for  Experiment  7,  R  =  2 
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Figure  22.      TOPC  Trajectories  for  Experiment  7,  R  =  3 
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Figure  25.   TOPC  Trajectories  for  Experiment  7,  R  =  20 


s 

118 

c 
■ 

o 

IP 

»"i 

c» 
to 

HJ 
o 

n 
o- 

UJ 

in 

■s. 
a 

IX 

Kg 

o 

o 
ru 

a 

o 

?■ 

I 



J"~ 

i 

..... 

i 

i 

L- 

i 

! 

1 

----- 



— t— 

_|_ 

1 

..     1    .    .    . 

1 

j 

l 

i 

■  ■    *  ■■      ■■ 

— \j — 

*~1 

i 

1 
1 

! 

1 

' 

i 

i 
i 

| 

\ 

■ 

1  \ 
~Y~\ " 

j 

—j-— 

I 

1 

1 

| 

| 

__J 

4— 

- — 4- — 

i 

\ 

i     i 

i 
i 

—  -j  — ■- 

I 

1 

L-l 

--'--- 

1 

1 

1 

o 

■•-— 

— 

f — 

L ... 

1 

1 

1 
i 

1 

l- 

'a 

o 

00             o.ou 

0 

00               «••*                0.16                0.20                0.2U               0.M                olw               o!»                0 

go 

s 

o- 

j 

i 

1 

1 

*_ 

— i 

i 

i 

I 

h- 

8 

a. 

O 

o 

a 

i 
a 

CE 

cc 

a 

o 
■9 

i 
a 

j 

i 

j 

I 

t 

j 

| 

1 

| 

! 

1 

i 

i 

j 

1 

i 

! 

1 

XP 

! 

f 

i 

| 

I 

1 

1 

■ 

i 

1 

A 

— X... . 

f 

I 

j 

1 



1 

I 

\ 

\ 

; 

I 

1 

i 

—i — 

1 

j 

I 

i 

— .L... 

--; - 

| 

a 
w 

— ' f- 

— ] - 

1        1 

-.  -  I™     . 

—  : .. 

;    1    \ 

0M- ^ 

sec . 

Figure   25.      Continued 

0 

119 


8 

I 

1 

i 

1 

! 

i 

i ' 

o 

1 

! 

1 

1 

I 

j 

i 

1 

8 

i 

1 

j 

1 

i 

.....[..... 

1 .. 

__L 

, 1 

i 

i 

O 

L 

i  ^ 

t 

! 

J 

■ 

o 
o 

I 

I 

t 

1 

i 

1 

\ 

a 
o 

1 

I 

I 

i 

i 

j 

8 

i 

1 

8  " 

i 

1 

! 

! 

1 

3 

1 

t — 

1    ■      ' 

| 

i 

I 

| 

0.00 

0 

M 

0. 

)a 

0. 

12 

0. 

10 

a. 

N 

0. 

ZH 

0. 

■ 

0. 

12 

0. 

3* 

o.g 

| 

1           1 

i 

1           1 

1 

i 

i 

j 

I 

8 

\ 

I 

j 

1 

\  \ 

i 

f 

1 

8 

! 

i 

I 

i 

! 

j 

1 

t 

.... 

f 

1 

— i— - 

- — j — 

— -f — 

— L — 

— -f — 

h- 

j-— 

!   V 

~\ 

H 
3 

1 

1 

j 

1 

j 

\ 

■ 

! 

t 

1 

i 

1 

u 

/ 

• 

i 

I 

t 

1 

i 

— j- — 

J 

M 

|—  ■ 

b 

- 

1 

8 

o 

j 

1 

I 

1 

i 

i 

! 

j 

S 

i-j     - 

1 

1 

1 

i 
i 

j 

| 

1 

! 

1 

1 

1    1    I 

! 

! 

0                0. 

H 

o.c 

*J                 0. 

2                 0. 

0                0. 

0                0. 

"U               0., 

1 

a. 

2                0. 

1 

sec . 


Figure  26.   TOPC  Trajectories  for  Experiment  7,  R 
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As  shown  in  Figure  27  the  TOPC  in  the  slewing  mode  of  a  single 
link  manipulator  is  rather  insensitive  to  guantization  errors  in  the 
control  signal,  since,  as  can  be  seen  in  Figure  25,  even  an  unrealistic 
value  of   R  =  20  volts  (which  is  about  25%  of  the  rated  voltage)  could 
be  accepted  as  slewing  mode  control . 

Experiment  8.   The  program  FIN2  was  used  to  perform  a  similar  set  of 
experiments  to  those  in  Experiment  7  for  a  TOPC  of  a  double  link  manipu- 
lator.  Again,  the  values  of 

R=1,  2,  3,  5,  10,  20  and  50  volts 

were  used.   In  each  run,  the  joints   positions  and  velocities  at  the 
terminal  time  t  =  tf,  were  recorded.   Figures  28  through  41  represent 
the  joints  positions  and  velocities  and  the  actuators  currents  and 
guantized  voltages  for  various  values  of   R.   in  addition,  the  resulting 
terminal  position  errors  for  the  first  and  second  joints  were  plotted 
as  a  function  of   R  in  Figures  42  and  43  respectively. 

As  shown  in  Figures  42  and  43,  the  effect  of  guantization  error 
is  more  significant  in  the  double  link  case  than  that  of  a  single  link. 
Additionally,  it  should  be  noticed  that  the  second  link's  output  motion 
was  more  sensitive  to  the  guantization  errors.   This  was  probably 
because  of  the  fact  that  the  second  link,  having  its  joint  attached 
to  the  first,  and  not  to  a  fixed  support,  suffers  from  more  significant 
Coriolis  and  centrifugal  coupling.   It  is  shown  that  a  guantization 
level  of  10  v  (about  12%  of  the  second  actuators  rated  voltage  and  6% 
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Figure  30.   T0PC  Trajectory  for  1st  Link  in  Experiment  8,  R  =  2 
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Figure  31.   T0PC  Trajectory  for  2nd  Link  in  Experiment  8,    R  =   2 
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Figure   32.      TOPC  Trajectory   for    1st   Link   in   Experiment   8,    R   =   3 
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Figure  36.   T0PC  Trajectory  for  1st  Link  in  Experiment  8,  R  =  10 
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Figure  37.   T0PC  Trajectory  for  2nd  Link  in  Experiment  8,  R  =  10 
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Figure  38.   TOPC  Trajectory  for  1st  Link  in  Experiment  8,  R  =  20 
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of  the  first)  still  produced  an  acceptable  terminal  error  for  the  slewing 
mode  of  a  TOPC. 

It  is  expected  (as  demonstrated  in  these  experiments )  that  for  a 
six  DOF  manipulator  the  acceptable  quantization  level  would  be  much 
smaller  than  10  volts.   Nevertheless,  the  experiments  demonstrate  that 
an  efficient  slewing  mode  TOPC  is  possible, in  spite  of  imperfect 
implementations  of  the  FLDT. 

5.2   Asymptotic  Stability  for  Imperfect  FLDT 
In  this  section  we  prove  a  theorem  on  the  existence  of  a  linear 
feedback  controller  for  the  N-DOF  manipulator /actuators  system,  which 
guarantees  asymptotic  stability  of  the  closed  loop  system,  in  spite  of 
analytic,  partially  known  deviations  of  the  applied  FLDT  from  its  nominal 
value.   (See  [H-1 ]  for  the  case  without  actuator  dynamics.) 

In  view  of  the  FLDT  given  in  Equation  (3.1 .16)  it  is  possible  to 
express  it,  in  the  stationary  case,  as 


V  =  M(Y1  )    Z  +  b(Y)  (5.2.1  ) 


where 

M(Y1)    =   LC"1    J<2T,)  (5.2.2) 

-1      .•  • 

b(Y)    =   LC         {W  +  I  -    F2W  -    F2£  -    F2AY1    +    [A  -    CF      -    F   B]    Y 

+    [B   -    F2    J(Y1  )    +   J(Ylf    Y2)]    Y3>  (5.2.3) 

and  where   all   the  notations   of    Chapters    II  and    III  are  kept. 
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In  order  to  perform  the  FLDT  as  given  in  (5.2.1  )  the  functions 
M(Y  )   and  b(Y_)   have  to  be  perfectly  known,  i.e.,  the  system  dynamic 
model  has  to  be  perfectly  known.   This,  of  course,  never  happens  since 
the  model  parameters,  as  well  as  the  system  state,  are  never 
accurately  available. 

Suppose  that  due  to  parameter  uncertainties,  (e.g.,  nonrigid  links) 
modelling  errors  and  temperature  and  other  environmental  related  variations 
effects,  the  actual  FLDT  used  is 


*\       ** 


V  =  M(Y  )  Z  +  b(Y)  (5.2.4) 


where 


M(Y1  )  =  M(Y1  )  +  AM(Y1  )  (5.2.5) 


b(Y)   =  b(Y)  +  Ab(Y)  (5.2.6) 

and  where   AM(Y  )   and  Ab(Y)   are  the  errors  in   M(Y  )   and  b(Y) 
respectively.   In  this  case,  it  is  clear  that  the  expected  decoupling 
and  linearization  of  the  eguations  of  motion  will  not  occur  and  the 
effect  of  the  application  of  the  control ,  Z,    on  the  system  may  not 
produce  the  desired  behavior . 

Notice,  however,  from  Eguations  (5.2.2)  and  (5.2.3)  that  since 
L  and   C  are  diagonal  matrices  with  accurately  known  parameters  (the 
armature  inductances,  the  torgue  constants  and  the  gear  reduction 
ratios  of  the  DC  motors),  and  since   J(Y  )  the  symmetric  inertia  matrix 
of  the  manipulator,  contains  only  ± parameters  to  be  modelled, 
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whereas  b  ( Y^  )  contains  the  bulk  of  the  model  parameters ,  the  following 

assumption  is  acceptable. 

Assumption  A1 :     AM(Y  )  -  0     V  Y   e  RN. 

If  assumption  A1  is  used,  then  the  applied  FLDT,  V,  may  be 
rewritten  as  follows: 

V  =  M(Y1  )  [Z  +  d(Y)]  +  b(Y)  (5.2.7) 

where 


d(Y)  =  M-\V  •  Ab(Y)  {5.2.8) 


Notice  that  since   L,  C  and   J(^  )   arfe  all  nonsingular  matrices, 
M   ( Y^  )      exists  for  all   Y^  eRN,   and  d(Y)   is  defined  everywhere. 

Regarding  d (Y)   as  a  component  of  the  new  control   Z  as  in 
Eguation  (5.2.7)  it  is  clear  in  view  of  the  results  in  Chapter  III, 
that  applying  the  imperfect  FLDT  (5.2.7)  to  the  N-DOF  manipulator  system 
yields  the  following  dynamic  model  for  the  ecjuivalent  open  loop  system. 

9 

Y   =  Y 

-2   -3  (5.2.9) 

Y3  =  Z  +  d (Y) 

where   Y^  ,  Y_2 ,  Y_3   and   Y  are  as  defined  in  Eguations  (3.1.3)  and 
(3.1  .5). 


b  Y)  contains  at  least  an  order  of  magnitude  more  parameters  than 
in   J(Y1  )   including  parameters  which  are  hardly  known  such  as  friction. 
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Notice  that  for  each  i-th  degree  of  freedom  of  the  manipulator 
the  following  closed  loop  dynamics  is  obtained 

1 1     2i 

Y    =  Y 
2i    3i  (5.2.10) 

y,.  =  z.  +  d. (y) 

3i     l     l  — 

for   1  =  1 ,  2,  .  .  .  N. 

Since  d. (Y)  may  be  a  nonlinear  function  of  all  state  variables 

(at  least  not  constrained  to  be  a  function  of  the  i-th  degree  of 
freedom  state  variables);  the  system  is  no  longer  linear  nor  is  it 
decoupled  to  a   N  subsystem  as  in  Section  (3.1). 

Eguation  (5.2.10),  however,  may  be  viewed  differently.   Let 
d. (Y)     be  regarded  as  a  "disturbance"   acting  upon  the  "nominal"  system, 
defined  in  Eguation  (3.1.19).   Then  each  i-th  subsystem  is  a  linear 

(a  triple  integrator)  system,  decoupled  from  all  other  subsystems,  and 
a  problem  of  concern  is  now,  how  to  accomodate  the  disturbance  d. (Y), 
via  a  proper  feedback,  in  each  subsystem,  such  that  asymptotic  stability 
of  the  closed  loop  system  in  a  neighborhood  of  the  state  space  origin 
is  guaranteed . 

In  what  follows,  under  the  additional  assumption  (A2)  that  the 
disturbance  d(Y)   is  an  analytic   vector  function   in  all  its 


Since   M(Y1  )   and  b(Y)   are  analytic  functions,  it  is 
sufficient  to  use  an  analytic  model  for   b(Y)   in  order  to  satisfy 
assumption  A2 . 
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arguments,  we  prove  the  existence  of  a  linear  feedback  controller  which 
guarantees  asymptotic  stability,  at  the  origin  of  the  closed  loop 
system's  state  space. 
Assumption  A2   d(Y)   is  an  analytic  vector  fucntion  in  all  arguments. 

As  an  analytic  mapping,   d(Y)   possesses  a  convergent  Taylor 
expansion  about  the  origin,   Y  =  0_  as  follows. 

d(Y)  =  d(0_)  +  D  Y  +  d^Y)  (5.2.11  ) 

where 

d(0_)   is  a  constant  vector, 


3d(Y) 


D  = 


5Y  is  the  Jacobian  matrix  of  d  with  respect  to   Y, 

-  Y  =  0  ""  - 


and  d^CY)   is  the  remainder,  covnergent  power  series  in   Y,  beginning 
with  terms  of  at  least  second  order. 

Eguation  (5.2.9)  may  then  be  written  as 

• 

Y  =  Y 
-1    -2 

• 

Y  =  Y 

J  (5.2.12) 

I3  _  5  +  $.(0)    +    D  Y  +  ^(y) 

Theorem  5.1  .   There  exists  a  linear  dynamic  feedback  control  law  such 
that  the  system  (5.2.12)  is  asymptotically  stable  at  the  state  space 
origin  for  any  analytic  disturbance  d(Y)  with  a  given  linear  part. 
Further,  the  stabilizing  feedback  controller  form  is  given  in  Eguation 
(5.2.16a). 
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Proof.  The  proof  is  composed  of  three  major  steps.  In  the  first  step 
we  introduce  a  dynamic  controller  and  prove  that  it  stabilizes  the 
linear  part  of  the  system  (5.2.12).   In  the  second  step  we  "shift" 
the  controller  state  such  that  the  total  closed  loop  system  possesses 
an  equilibrium  point  at  the  origin.   Finally,  in  the  third  step,  we 
refer  to  a  theorem  by  Liaponov  to  show  the  existence  of  asymptotic 
stability  neighborhood  in  the  presence  of  d  (Y). 

—CO   Ml 

_A    The  linear  part  of  system  (5.2.12)  is  given  by 


FY  +  GZ 


(5.2.13) 


where 


F  = 


0 

I 

0 

0 

0 

I 

D1 

D2 

D3 

G  = 


(5.2.14) 


and 


(Dr  D2,  D3)   i.e. 


3d(Y) 

3Y. 

—l 


(0) 


i  =  1,  2,  3 


(5.2.15) 


Let  the  stabilizing  control  law  be  given  by   (5.2.16) 


£  =  It  +  K   I 


(5.2.16) 


The  choice  of  the  controller  form  is  not  unique.   The  above 
given  form  was  chosen  because  of  its  simplicity. 
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where   K  =  (K  ,  K  ,  K  )      is  yet  to  be  specified,  and   Y   the 

NX3N  ~C 

controller  state  satisfies: 


«C--X,  -ll'h'    <-*■  -1'  "«  I 


(5.2.17) 


By  closing  the  loop  (i.e.,  applying  (5.2.16)  to  (5.2.13))  we  obtain 


Y  Y 

* 
=  F 

Y„  Y 

C  C 


(5.2.18) 


where 


0 

I 

0 

0 

* 

F      = 

0 

0 

I 

0 

K1 

+ 

D1 

K2 

+ 

D2 

K3    +    D3 

I 

-I 

-I 

-I 

0 

(5.2.19) 


To  complete  the  first  step  we  only  need  to  show  that  F  is  a 
stable  matrix  (i.e.,  all  eigenvalues  of  F  are  in  the  left  half  of 
the  complex  domain 
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SI                      -I                      0                        0 

|SI    -    F*|    = 

0                          SI                       -I                       0 

-VD1              -VD2           SI-K3-°3        -1 

I                          I                          I                       SI 

- 

=    |SI(SI[(SI   -    K     -    D    )    SI   +    I]    +   I   -    SI(K 
s           o                                                     2 

+   D2)} 

r 

+  I  -  SICKj    +  D  )| 

or 

I  SI    -    F*|    =    |S4I    -    S3(K      +    D    )    +    S2(I    -    r      _    D    )    + 

•3            J                                  2.            2 

■ 

+   S(I    -    ^     -    D1  )    +    1 1 

(5.2.20) 

Recall   that     K  =    (^ ,    K2 ,    K3 )      is  yet  to   be   specified,    and 

that 

D     is   the   linear  part  of   the  distrubance,    thus  choose      K   , 

K2,    K3 

such  that 

K3    =    -41    -    D3 

• 

K      =    _5I    _    D. 

z. 

(5.2.21  ) 

K,    =    -31   -    D 

Equation    (5.2.20)   with  the  choice  of      K     in    (5.2.21)   yields 

|  SI   -    f*|    =    |S4I    +   4S3I    +  6S2I   +  4SI    +    l|    = 

=    (S4    +  4S3    +  6S2    +  4S   +   1)N  =    (S  +   1)4N 

i.e.,    all      4N     eigenvalues   of      F*     are   at      S  =   -1      and      F* 

is 

therefore   a  stable  matrix. 
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B     Utilizing  the  control  law  given  in  (5.2.16)  and  (5.2.17)  in  the 
system  (5.2.12)  when  only  d(0)  and  DY  terms  are  present  yields 


Y  =  Y 
-1    -2 

Y  =  Y 
-2    -3 


Y3  =  YC+(K+D)Y+  d(0_) 


(5.2.22 


Y   =-Y   -Y   -Y 

-C     -1    -2    -3 


Ihis  system  possesses  an  equilibrium  point  at  (simply  set  Y  =  Y  =  0) 


Y  =  0 


Yc=  -d(O) 


If,  however,  one  defines 


Ic  =  ^C  +  -(0) 


(5.2.23) 


then  the  overall  dynamics  of  the  system  may  be  described  by 


0 

Y 

0 

-N 

+ 

}c_ 

I 

0 

-    J 

d  (Y) 


(5.2.24) 
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but  now  (5.2.24)  has  an  equilibrium  point  at  the  state  space  origin. 


y  =  o 

C    The  controller  introduced  in  (5.2.16)  and  (5.2.17)  was  shown  to 
make  the  linear  part  (5.2.13)  of  the  system  stable.   Also, for  equilibrium 
at  the  origin  it  is  required  to  use  a  controller  of  the  form 

Z  =  *c  +  KI  (5.2.16a) 

instead  of  (5.2.16) 

We  shall  now  state  a  theorem  by  Liapounov  which  is  required  for 
completion  of  the  proof . 
Theorem   (Liapounov  [M-3]  pp.  87  -  88) 

Given  the  system 

X  =  AX  +  £(X ) 

If   A  is  a  stable  matrix  and  fp(X)   is  a  converqent  power  series  in 
X   beqinning  with  terms  of  at  least  the  second  degree,  then  the  origin 
X  =  0   is  stable  for  any   q>(X). 


This  was  a  major  reason  why  the  controller  form  (3.4.18),  (3.4.19) 
was  chosen.   If  the  equilibrium  point  'location'  is  irrelevant,  a  more 
qeneral  dynamic  controller  may  be  introduced  and  the  existence  of  the 
stabilizinq  controller  may  be  proved  by  simple  reference  to  the  pole 

^qq  -i  rfnmpnt 


assignment 
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*C 


It  is  easy  to  see  now  that  since  in  (3.4.26)   F*  is  stable,  and 


is  an  equilibrium  point  and  since  d  (Y)   is  such  that 


d  (0)  =  0 


3d 

— a 

6y" 


=  0 


Y  =  0 


then  the  state  space  origin 
any  d  ( Y ) . 

■  CD  — 


*C 


=  0   is  asymptotically  stable  for 


Q.E.D 


CHAPTER  VI 
CONTROLLERS  STRUCTURES,  CONCLUSIONS  AND  FUTURE  WORK 

Based  upon  the  results  of  the  previous  chapters,  we  discuss  in 
this  chapter  various  controllers  structures  to  carry  out  the  desired 
task.   Then  the  dissertation  conclusions  as  well  as  suggestions  for 
future  work  are  listed. 

6.1   Controller  Structures 
Figure  44  describes  the  structure  of  common  controllers  employed 
today  (see  for  example  [P-3],  [P-2]  and  [B-4].   Each  joint  has  essen- 
tially a  separate  servo  with  the  feedback  gains   K   and   K   for  rate 
and  position  respectively  being  selected  via  the  so-called  "worst  case 
design."   The  effect  of  nonlinearities  and  coupling  terms  is  compensated 
by  a  feedforward  estimator  based  upon  the  measurements  of  the  other  joints ' 
positions  and  rates.   Frequently,  the  joints  accelerations  are  estimated 
from  those  measurements  by  digital  differentiation.   The  desired 
terminal  position   9.   desired  for  each  joint  is  obtained  with  the  aid 
of  a  trajectory  planner  which,  in  fact,  utilizes  the  manipulator's 
geometrical  mapping  to  determine  the  desired  joint  angles  from  the 
desired  position  and  orientation  of  the  end-effector.   As  already 
mentioned  i„  Chapter  I,  the  desired  end  effector  path  in  point  to  point 
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type  tasks  is  arbitrarily  (usually  by  interpolating  intermediate  points 
along  a  straight  line)  selected.   Similarly  the  servo  gains   K   and 
K2   for  each  joint  are  selected  under  worst  case  conditions  indepen- 
dently of  the  other  joint's  dynamics.   As  a  result,  the  manipulator's 
trajectory  between  the  assigned  points  is  unpredictable  and  clearly 
not  optimal  with  respect  to  any  preassigned  criteria.   Moreover,  the 
currently  used  control  schemes  do  not  take  into  account  state  variable 
constraints  such  as  maximum  velocity  acceleration  and  jerk  of  the 
joint's  motion. 

Based  upon  the  results  of  the  previous  chapters,  we  describe  the 
suggested  controllers  for  the  time  optimal  positioning  control  of 
N-DOF  manipulators  taking  into  account  the  constraints  on  the  velocity 
acceleration  and  jerk  of  each  joint.   Figure  45  describes  the  open 
loop  controller  with  references  to  the  governing  eguations .   Figure  46 
describes  the  mathematical  eguivalent  of  this  controller.   As  shown  in 
Chapter  III,  the  result  of  applying  the  FLDT  is  that  the  optimal  con- 
trol (and  the  resulting  motion)  of  each  DOF  of  the  manipulator  is 
uncoupled  from  the  others.   This  allows  for  the  separate  design  of 

the  controllers  for  each  DOF.   In  an  open  loop  mode  each   Z*(t)   in 

i 

Figures  45  and  46  is  given  by  Equation  (4.3.30).   The  exact  scheme  of 
Figure  45  was  used  in  the  Experiments  described  previously. 

In  the  case  of  a  closed  loop  form,  that  is,  when  the  optimal  jerk 
* 
input   Z  (t)   is  expressed  as  a  function  of  the  state  variables,  we 

obtain  the  controller  in  the  form  described  in  Figure  47,  or  in  its 

mathematically  eguivalent  form  in  Figure  48. 
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In  the  case  of  no  constraints  (i.e,  unconstrained  velocities, 
accelerations  and  jerks)  the  form  of  the  i-th  time  optimal  position 
controller  (TOPC.  in  Figures  47  and  48)  is  that  of  bang-bang  controllers 
according  to  the  switching  surface  function  F  (found  in  [F-1]  and 
[S-3])  of  Equation  (4.3.6).   This  form,  however,  is  quite  complicated 
for  analog  devices  implementation  and  successful  efforts  were  made  to 
approximate  F  by  least  square  fitting  points  on  the  optimal  switching 
surface  with  a  simple  linear  segment  switching  surface  [S-3]  (see  also 
[C-1]  for  the  second  order  system  case).   Another  approach  [R-2] 
exploits  a  scaling  property  of  the  optimal  system  which  enables  its 
dynamic  behavior  to  be  depicted  in  a  two-dimensional  reduced  state 
space. 

However,  in  the  case  of  time  optimal  control  of  a  triple  integrator 
with  state  variables  inequality  constraints  (which  is  our  case),  no 
related  work  could  be  found  in  the  literature,  and  the  open  loop  optimal 
control  was  obtained  in  Chapter  IV.  The  state  feedback  (closed  loop) 
form  for  a  TOPC  problem  may  be  found,  by  exploiting  the  fact  that  the 
time  optimal  joint  angle  trajectory  is  a  strictly  monotonous  function 
of  time  (see  Figure  11)  and  thus  a  simple  feedback  function  Z*(6  ) 
could  be  found  from  Equations  (4.3-30)  through  (4.3.33)  as  follows. 
Since 
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P2   9(t2}  +  5  = 


2A 


u) 


6A 


_M 

2a 


(6.1  .2! 


M 


p.,  =  e(t.)  +  1   =  -2  (_2  +  _2  ) 

M 


(6.1  .3) 


where   t  ,  t   and  t   are  the  switching  times  given  in  Eguation  (4.3.34). 
then,  due  to  the  symmetry  of  the  path  trajectory  with  respect  to  — 
we  find  that 


* 
Z  (0) 


A 
0 

-A 
0 

-A 
0 


<   -  0  <  P.   -5 


p1  -  5  -  e  <  p,  -  5 


p2  "  5  -  e  <  p3  -  K 


p3  -  e  -  e  <  -  p3 


-p3  -  e  <  -  p2 


"p2  "  9  <  "  P1 


*,  K-  e  <  0 


(6.1  .4) 


Eguations  (6.1  .1  )  through  (6.1  .4)  defind  the  algorithm  for  each  TOPC 

i 

block  in  Figures  47  and  48 . 

As  shown  in  Eguation  (6.1.4),  only  position  measurements  (9  ) 

i 

are  reguired  for  implementing  the  TOPC  algorithm.   It  is  well  known, 
however,  that  in  switching  function  controllers,  time  delays  and  in- 
accuracies in  the  controller  introduce  the  so-called  "chattering"  mode 
phenomena,  in  which  case,  the  angular  rates  may  be  found  helpful  in 
improving  the  controller  performance.   As  can  be  seen  in  Figure  47, 
since  the  FLDT  reguires  the  measurements  of  positions  and  rates,  these 
signals  are  available  at  no  "extra  cost"  to  the  TOPC  blocks. 
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6.2   Conclusions  and  Future  Work 

The  following  conclusions  may  be  obtained  from  this  dissertation. 

A  linearizing  and  decoupling  feedback  transformation  which  reguires 
no  matrix  inversion  may  be  found  for  the  general  case  of  a  deterministic 
model  for  a  N-DOF  manipulator  including  its  actuator's  dynamics,  gravity 
and  known  loads  effects . 

It  was  shown  that  the  time  optimal  positioning  control  of  N-DOF 
manipulator  could  not  be  directly  solved  via  present  theory  of  optimal 
control,  furthermore  the  present  time  optimal  trajectories  and  control 
are  not  unigue  (undetermined)  if  the  common  time  optimization  problem  is 
posed . 

An  analytic  (closed  form)  control  law  for  the  restricted  time  optimal 
positioning  control  (TOPC)  problem  of  a  manipulator  satisfying  various 
state  variable  ineguality  constraints  was  found  via  the  feedback  linearizing 
and  decoupling  transformation  (FLDT).   Due  to  the  resulting  uncoupled 
behavior  of  the  manipulators'  degrees  of  freedom  the  TOPC  law  with  SVICs 
is  simple  and  easy  to  program  for  each  joint  independently.   Moreover, 
the  resulting  optimal  trajectory  and  control  are  unigue. 

The  trapezoidal  acceleration  curves  obtained  for  each  joint  as  a 
result  of  minimum  time  dynamic  optimization  with  SVICs  are  identical  to 
those  recommended  by  researchers  for  minimizing  the  amplitude  of  vibratory 
transients  in  flexible  structure,  indicating  high  correlation  between 
these  two,  seemingly  independent  design  criteria. 

The  TOPC  with  SVICs  is  shown  not  to  be  a  torgue  (current)  "bang -bang" 
control,  which  had  been  assumed  in  previous  studies. 
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The  simultaneous  time  suboptimal  control  of  all  DOF  of  the 
manipulator  via  current  approximated  voltage  "bang -bang"  control  was 
shown  to  be  unsuccessful  due  to  the  coupling  effects  of  the  manipulator's 
dynamics  . 

Realization  of  the  FLDT  through  table  look-up  which  necessarily 
introduces  guantization  errors  into  the  control  was  shown  to  be 
promising.   It  was  found  that  for  a  double  link  manipulator  a  high 
guantization  level  (10  V)  was  permissible  in  the  "slewing  mode"  con- 
trol.  This  may  imply  the  feasibility  of  table  look-up  methods  as  a 
FLDT  realization  technigue  for  on-line  optimal  control  "in  the  large"  of 
robotic  manipulators . 

The  destabilizing  effects  of  imperfect  modelling  and  general 
analytic  deviations  in  realization  of  the  FLDT  are  shown  not  to  be 
prohibitive  since  a  linear  feedback  control  always  exists  which  guaran- 
tees closed  loop  asymptotic  stability.   (The  linear  part  of  the  derivation 
function  was  assumed  to  be  given.) 

Due  to  the  resulting  linear,  low  order  (3),  uncoupled  dynamic 
behavior  of  each  DOF  of  the  manipulator  when  the  FLDT  is  employed, 
additional  performance  criteria  optimizations  (in  addition  to  time 
optimization)  may  be  attempted,  (e.g.,  TOPC  with  actuator  energy  losses 
constrained)  including  the  multicriteria  case  where  each  or  several 
manipulator  joints  are  controlled  to  satisfy  various  performance 
criteria  simultaneously,  e.g.,  lower  (heavier)  joints  will  have  actua- 
tor energy  losses  constraints  whereas  the  lighter  links  (hand)  will 
be  controlled  via  TOPC. 
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Various  controllers  structures  are  suggested  for  realization  of 
the  control  laws  found . 

The  possibility  of  employing  the  FLDT  in  order  to  solve  other 
manipulator  control  problems  such  as  tracking  ("contouring")  has  to 
be  studied.   FLDT  facilitates  the  solution  of  the  tracking  problem. 
The  resulting  uncoupled  dynamics  when  the  FLDT  is  applied  may  signifi- 
cantly facilitate  the  incorporation  of  sensors  data  (obtained  from  the 
"hand")  in  parallel  processing  at  joints'  levels. 


APPENDIX  I 
EQUATIONS  OF  MOTION  FOR  A  SINGLE  LINK  MANIPULATOR 

In  this  Appendix  the  equations  of  motion  for  a  single  link 
manipulator  driven  by  a  DC  motor  are  derived,  using  the  Lagrangian 
method . 

From  Figure  4,  the  potential  energy  of  the  system  is 
P  =  -  mgd  cos0 

and  the  kinetic  energy  is 

2  ? 

K  =  ijmd  9 

thus   the    Lagrangian,    L,    is  given  by 

2    2 
L  =   K  -   P  =  Sjmd   9      +  mgd  cos9 

The  generalized  torgue,  T,  at  the  link's  joint  is  given  by 


_  d_  6L        9L 

dt  ae~"~  ae 

Here,    we  obtain 

D& 

3L 

gg  =  -  mgd  sin9 


— -  =   md  9 

ag 
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and 


d    9L     *2a 

—  —  =  md  9 

dt  ae 


thus 


2  " 

T  =  md  9  +  mgd  sin6  (A1 -1  ) 


Summation  of  torques  at  the  output  shaft  yields 


2"  2" 

JMr9  =  rKI-T  =  rKI-mde-  mgd   sin9 


or 


9  =    [rKjI-  mgd   sin9]    -  (A1-2) 

where 

2  2 

J  =  md      +  J     r    . 
m 

Additionally,  we  obtain  for  the  motor 

LI  +  RI  +  r  Kv  9=V  (A1_3) 

Finally,  rewriting  (A1-2)  and  (A1-3)  in  state  variables  form, 
we  obtain 

9  =  0) 

T      mgd 

10  "■j      X  "   j   slnQ  (A1-4) 

•      R  T    r  KV      V 
I  =  -  -  I  -r u  +  - 

L        Li  L 

Equations  (A1-4)  are  the  state  space  model  for  this  system. 


APPENDIX  II 
EQUATIONS  OF  MOTION  FOR  A  2 -LINK  MANIPULATOR 

In  this  Appendix  the  equations  of  motion  for  a  2-link  manipulator 
are  derived  based  upon  the  Lagrangian  method. 

From  Figure  6  we  obtain  the  following  expressions  for  the  kinetic 
and  potential  energies  of  the  system   K  .   The  kinetic  energy  of  the 
first  link  is 


■"•2 
K1  -  *"ldiei  (A2-1) 


P1  ,  the  potential  energy  of  the  first  link  may  be  expressed 


as 


P1  =  -m1gd1  cos91  (A2_2 


For  the  second  link  we  will  first  write  expressions  for  the  Cartesian 
position  coordinates  and  then  differentiate  them  in  order  to  obtain 
the  velocity 


d1sin61  +  d2  sin (6   +  9  ) 


Y9  =  -  d  cos0   -  d.  cos (9,  +  9 
«      112       12 
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The   Cartesian  components   of   velocity   are   then 

x*2  =  d1  cos(e1 )  ^  +  d2  cos(e1  +  e2)  (e    +  e  ) 

Y2   =  d1    sin(61 )   ^    +  d2   sin(91    +  ©2 )    ($     _  e    ) 

Thus  the  second  link  kinetic  energy  is 

K2    =%m2(X2    +Y2)2 
or 

K2  =  ^1*1  +  ^^i  +  V2  +  m2did2  c°s(e2)(^  +  e,  e2)         (A2-3) 

The  potential  energy  of  the  second  link  is 


P2  =  m2g  y2 


or 


P2   =   -  m2g  d1    cos91    -  m2g  d2   cos(91    +  92 )  (A2_4) 

This    Lagrangian      L  =   K  -   p     is   obtained   from   Equations    (A2-1  )    - 
(A2-4)    since 


K  =   *,     +   K2 


and 


p-g, 


+   P„ 


thus, 
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L--  him^  +  m2)  d1ei  +  te2d2i01  +  e2)2  +  »2dtd2  cos(92)(92  +  e  e  >  + 

+    (ra1    +  m2)   g  d.|    cos(91  )    +  m2gd2   cos(91    +  9    )  (A2-5) 

According  to   the    Lagrangian  method  the   eguations  of  motion   are 
obtained  by 

d     3L  3L                                                 * 

Ti    -  dt  ~  ~  ~                                  i    =   1,2 

39.  39 

l  l 

where   TV   is  the  i-th  generalized  torgue .   Therefore,  in  order  to 
obtain  the  dynamics  eguations  we  differentiate  the  Lagrangian   L  as 
follows 

3L  2«       2*       2*  . 

—  =  (m1  +  m2)  d19l  +  m2d291  +  n^d^  +  2m2d1d2  cos (6  )  8   + 


891 


+  m2d1d2   c°s(9    )    62 


d     1L  ..  2  2 

—  — —  =    [(m^    +  m2 )   d1    +  m2d2    +  2m2d1d2   cos (9    )  ]    9      + 


"1 


2 
+    [m2d2    +  m2d1d2   cos(02)]    92    + 


-   2m2d1d2   sin(92)    9^    -  *gx&2   sin(92)    gjj 


3L 

=   -    (m      +  m    )   gd      sin (9    )    -  m.gd„    sin (9.    +  9_ ) 
39  i/i  122  12 


thus , 
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2  2 

T1    =   C(m1   +  m2)   d1   +  m^  +  2m2<i^d2  cos(G2)]   8     + 

2 
+  [m2d2  +  "2did2  cos(e2^  62  + 

-  2m2d1d2  sin(62)  S^  -  m2d1d2  sin(6  )  9^  + 

+   (m1  +  m2)  gd1  sinCG^  +  m2gd2  810(6'  +  6  )  (A2-6) 

Similarly  for  the  second  link  we  find 


8L       ?•       2* 

—  -  m2d291  +  m2d292  ♦  m2dld2  COs(92)  B^ 


d 3L       ?"  2"  "  .    . 

dt~~T  ■  m2d2G1  +  m2d292  +  m2d1d2  cos(02)  61  "  m2d1d2  sin(62)  6i92 


^-   6L 


ae2 


m2d1d2  sin(92)  ®1®2  "  m2gd2  sin(61  +  e2)  + 


-  m2d1d2  sin(92)  9^ 

thus 

2  ..       _  .. 

T2  =  [m2d2  +  m2d1d2  cos(62)]  Bi    +   m2d2  62  + 

+  m2d1d2  sin(92)  ^  +  m2gd2  sin^  +  e£)  (12-7.3 

Equations  (A2-6)  and  (A2-7)  represent  the  equations  of  motion  of  the 
2-link  system. 


APPENDIX  III 
COMPUTER  LISTINGS 
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